def test_append_empty(self):
pl = Polyline()
v = []
with raises(ValueError):
pl.append_vertex(v)
return
def test_distance(self):
v = [[1, 3], [3, 5], [6, 2], [2, 2]]
pl = Polyline(v)
# Test values are chosen to yield simple distances and vectors and were
# calculated by hand apriori
pts = [[1, 2], [1.5, 5.5], [3, 6], [3.5, 3.5], [5.5, 3.5], [4, 1],
[2, 2.5]]
dists, _, proj_to, proj_vecs = pl.get_dist_to_line(pts)
# Should show from pt to closest part of the polyline
proj_vecs_exp = [[0, 1], [1, -1], [0, -1], [0.5, 0.5], [-0.5, -0.5],
[0, 1], [0, -0.5]]
dists_exp = [1., sqrt(2.), 1., sqrt(2.) / 2., sqrt(2.) / 2., 1., 0.5]
# id < 0: Onto segment with -id-1, else onto vertex with id
# Last point projects exactly orthogonally to end of segment, thus -3
proj_to_exp = [0, -1, 1, -2, -2, -3, -3]
for i, (pvexp, dexp,
ptexp) in enumerate(zip(proj_vecs_exp, dists_exp,
proj_to_exp)):
assert proj_vecs[i, 0] == approx(pvexp[0])
assert proj_vecs[i, 1] == approx(pvexp[1])
assert dists[i] == approx(dexp)
assert proj_to[i] == ptexp
return
def test_angle_gradient_diag(self):
# Middle index is moved on a fine grid to test analytical dx, dy
# components againt the numerical ones from finite differences
pl = Polyline([[-1.03, -1.03], [0, 0], [1.03, 1.03]])
idx = 1 # Movable vertex id (middle vertex to obtain mutliple angles)
test_eps = 1e-2 # Max allowed deviation, chose suitable for num grad
# Test on a diagonal between fixed vertices
x_pos = np.linspace(-2, 2, 501)
cos_angles, cos_angles_grad_x, cos_angles_grad_y = [], [], []
for xi in x_pos:
pl.replace_vertex(idx, [xi, -xi])
cos_ang, cos_ang_grad = pl.get_cos_angle_at_vertex(idx)
cos_angles.append(cos_ang)
cos_angles_grad_x.append(cos_ang_grad[0])
cos_angles_grad_y.append(cos_ang_grad[1])
# Test analytical against numerical gradients
cos_angles_grad = (np.array(cos_angles_grad_x) -
np.array(cos_angles_grad_y)) / sqrt(2)
num_grad = np.gradient(cos_angles, np.diff(x_pos)[0] * np.sqrt(2))
assert np.allclose(num_grad, cos_angles_grad, atol=test_eps)
return
def draw_phrase(self, phrase: str, x: int, y: int):
self.cursor = Polyline(x, y)
self.phrase = phrase.upper()
for letter in self.phrase[:-1]:
self.draw_letter(letter)
if letter is not " ":
self.cursor.go_right()
self.draw_letter(self.phrase[-1])
def test_insert_single_shape(self):
pl = Polyline()
v = [1, 2] # Single vertex in non-2D form should also be accepted
pl.insert_vertex(v, 0)
assert pl.nvertices == len(pl.vertices) == 1
assert len(pl.segment_lengths) == pl.nsegments == 0
assert pl.vertices[0, 0] == v[0]
assert pl.vertices[0, 1] == v[1]
return
def test_append_one_vertex(self):
pl = Polyline()
v = [[1, 2]]
pl.append_vertex(v)
assert pl.nvertices == len(pl.vertices) == 1
assert len(pl.segment_lengths) == pl.nsegments == 0
assert pl.vertices[0, 0] == v[0][0]
assert pl.vertices[0, 1] == v[0][1]
return
def test_distance_point_grad(self):
# Test gradient with respect to each projected point, leaving the
# polyline fixed.
# Move pts along simple polyline and compare gradients.
# Points are a bit tunes, because at discontinuous points, the numerical
# gradient jumps to far for the tested precision, although the plots are
# perfectly ok
pl = Polyline([[0, 0], [1, 0.8], [1.1, 1.9]])
test_eps = 1e-2 # Max allowed deviation, chose suitable for num grad
npts = 1000
# Test x gradient
ys = [-1, 2]
x = np.linspace(-1, 2, npts)
for yi in ys:
y = yi + np.zeros_like(x)
pts = np.vstack((x, y)).T
dists, _, _, proj_vecs = pl.get_dist_to_line(pts)
# Gradients are simply the normalized negative projection vectors
_norm = np.linalg.norm(proj_vecs,
axis=1).reshape(len(proj_vecs), 1)
gradients = -proj_vecs / _norm
# Directional gradient along x
num_grad = np.gradient(dists, x)
grad = np.dot(gradients, [1, 0])
assert np.allclose(num_grad, grad, atol=test_eps)
# Test y gradient
xs = [-1, 2]
y = np.linspace(-1, 1.5, npts)
for xi in xs:
x = xi + np.zeros_like(y)
pts = np.vstack((x, y)).T
dists, dists_grad, _, proj_vecs = pl.get_dist_to_line(pts)
# Gradients are simply the normalized negative projection vectors
_norm = np.linalg.norm(proj_vecs,
axis=1).reshape(len(proj_vecs), 1)
gradients = -proj_vecs / _norm
# Directional gradient along x
num_grad = np.gradient(dists, y)
grad = np.dot(gradients, [0, 1])
assert np.allclose(num_grad, grad, atol=test_eps)
return
def test_angle(self):
# Test values chosen to yield simple angles: 180°, 135°, 90°, 45°, 0°
v = [[0, 0], [1, 1], [2, 2], [2, 3], [1, 3], [2, 4], [1, 3]]
pl = Polyline(v)
angles = [
pl.get_angle_at_vertex(i)[0] / pi * 180.
for i in range(1, pl.nsegments)
]
angles_exp = [180., 135., 90., 45., 0.]
for ang, ang_exp in zip(angles, angles_exp):
assert ang == approx(ang_exp, abs=1e-5)
return
def test_append_two_vertices(self):
pl = Polyline()
v = [[1, 2], [3, 4]]
pl.append_vertex(v)
assert pl.nvertices == len(pl.vertices) == 2
assert len(pl.segment_lengths) == pl.nsegments == 1
for i, vi in enumerate(v):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
assert pl.segment_lengths[0] == approx(
norm(v[0][0], v[0][1], v[1][0], v[1][1]))
return
def setUp(self) -> None:
self.p = Polyline(Point(1, 2),
Point(2, 3),
Point(3, 4),
Point(4, 5),
lineColor="pink",
backgroundColor="green")
def test_append_many_vertices(self):
pl = Polyline()
v = [[1, 2], [3, 4], [4, 5], [7, 8], [9, 10]]
pl.append_vertex(v)
assert pl.nvertices == len(pl.vertices) == len(v)
assert len(pl.segment_lengths) == pl.nsegments == len(v) - 1
for i, vi in enumerate(v):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
if i < len(v) - 1:
assert pl.segment_lengths[i] == approx(
norm(vi[0], vi[1], v[i + 1][0], v[i + 1][1]))
return
def test_constructor_empty(self):
v = []
pl = Polyline(v)
assert pl.nvertices == len(pl.vertices) == 0
assert len(pl.segment_lengths) == pl.nsegments == 0
return
def test_insert_in_len_one_polyline(self):
v = [[1, 2]]
pl = Polyline(v)
v_ins = [[3, 4]]
pl.insert_vertex(v_ins, 0) # Insert at beginning
pl.insert_vertex(v_ins, 2) # Insert at end
assert pl.nvertices == len(pl.vertices) == 3
assert len(pl.segment_lengths) == pl.nsegments == 2
for i in [0, 2]:
assert pl.vertices[i, 0] == v_ins[0][0]
assert pl.vertices[i, 1] == v_ins[0][1]
assert pl.vertices[1, 0] == v[0][0]
assert pl.vertices[1, 1] == v[0][1]
return
def test_remove_vertex_end(self):
v = [[1, 3], [3, 5], [6, 2], [2, 2]]
pl = Polyline(v)
pl.remove_vertex(len(v) - 1)
v_expected = v[:-1]
assert len(pl.vertices) == pl.nvertices == len(v_expected)
assert len(pl.segment_lengths) == pl.nsegments == pl.nvertices - 1
for i, vi in enumerate(v_expected):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
if i < pl.nsegments:
assert pl.segment_lengths[i] == approx(
norm(vi[0], vi[1], v_expected[i + 1][0],
v_expected[i + 1][1]))
return
def test_replace_vertex_middle(self):
v = [[1, 2], [3, 4], [5, 6], [7, 8], [9, 10]]
pl = Polyline(v)
v_repl = [-1, -1]
pl.replace_vertex(2, v_repl)
v_expected = v[:2] + [v_repl] + v[3:]
assert len(pl.vertices) == pl.nvertices == len(v_expected)
assert len(pl.segment_lengths) == pl.nsegments == pl.nvertices - 1
for i, vi in enumerate(v_expected):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
if i < pl.nsegments:
assert pl.segment_lengths[i] == approx(
norm(vi[0], vi[1], v_expected[i + 1][0],
v_expected[i + 1][1]))
return
def test_segment_length(self):
# Test values chosen to yield simple lengths
v = [[0, 0], [0, 1], [1, 1], [1, 3], [3, 3], [4, 4]]
pl = Polyline(v)
lens = pl.segment_lengths
lens_exp = [1., 1., 2., 2., sqrt(2)]
for li, l_exp in zip(lens, lens_exp):
assert li == approx(l_exp, abs=1e-5)
def test_insert_between_scale_1(self):
v = [[1, 0], [1, 1]]
pl = Polyline(v)
pl.insert_vertex_between(1, scale=1)
v_expected = v + v[1:]
assert pl.nvertices == len(pl.vertices) == len(v_expected)
assert len(pl.segment_lengths) == pl.nsegments == pl.nvertices - 1
for i, vi in enumerate(v_expected):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
if i < pl.nsegments:
assert pl.segment_lengths[i] == approx(
norm(vi[0], vi[1], v_expected[i + 1][0],
v_expected[i + 1][1]))
return
def test_constructor_one_vertex(self):
v = [[1, 2]]
pl = Polyline(v)
assert pl.nvertices == len(pl.vertices) == 1
assert len(pl.segment_lengths) == pl.nsegments == 0
assert pl.vertices[0, 0] == v[0][0]
assert pl.vertices[0, 1] == v[0][1]
return
def test_remove_vertex_middle_2(self):
# Same as above, but more than two leftover vertices
v = [[1, 3], [3, 5], [6, 2], [2, 2], [3, 3], [4, 4]]
pl = Polyline(v)
# Order matters
pl.remove_vertex(2)
pl.remove_vertex(1)
v_expected = v[:1] + v[3:]
assert len(pl.vertices) == pl.nvertices == len(v_expected)
assert len(pl.segment_lengths) == pl.nsegments == pl.nvertices - 1
for i, vi in enumerate(v_expected):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
if i < pl.nsegments:
assert pl.segment_lengths[i] == approx(
norm(vi[0], vi[1], v_expected[i + 1][0],
v_expected[i + 1][1]))
return
def test_angle_gradient_dy(self):
# Middle index is moved on a fine grid to test analytical dy components
# againt the numerical ones from finite differences
pl = Polyline([[-1.03, -1.03], [0, 0], [1.03, 1.03]])
idx = 1 # Movable vertex id (middle vertex to obtain mutliple angles)
test_eps = 1e-2 # Max allowed deviation, chose suitable for num grad
x_pos = [1.5, 0.75, 0.25, 0., -0.25, -0.75, -1.5]
y_pos = np.linspace(-2, 2, 501)
for xi in x_pos:
cos_angles, cos_angles_grad_y = [], []
for yi in y_pos:
pl.replace_vertex(idx, [xi, yi])
cos_ang, cos_ang_grad = pl.get_cos_angle_at_vertex(idx)
cos_angles.append(cos_ang)
cos_angles_grad_y.append(cos_ang_grad[1])
# Test analytical against numerical gradients
num_grad_y = np.gradient(cos_angles, y_pos)
assert np.allclose(num_grad_y, cos_angles_grad_y, atol=test_eps)
return
def test_insert_multiple_random(self):
v = [[1, 2], [3, 4], [5, 6], [7, 8]]
pl = Polyline(v)
v_ins = [[9, 10], [11, 12], [13, 14]]
pl.insert_vertex(v_ins, 2)
pl.insert_vertex(v_ins, 5)
v_expected = v[:2] + v_ins + v[2:]
v_expected = v_expected[:5] + v_ins + v_expected[5:]
assert pl.nvertices == len(pl.vertices) == len(v_expected)
assert len(pl.segment_lengths) == pl.nsegments == pl.nvertices - 1
for i, vi in enumerate(v_expected):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
if i < pl.nsegments:
assert pl.segment_lengths[i] == approx(
norm(vi[0], vi[1], v_expected[i + 1][0],
v_expected[i + 1][1]))
return
def test_insert_in_empty(self):
pl = Polyline()
v = [[1, 2]]
with raises(ValueError):
pl.insert_vertex(v, 1) # 1 is out of bounds for empty polyline
with raises(ValueError):
pl.insert_vertex(v, -1) # -1 is out of bounds
pl.insert_vertex(v, 0) # Proper insert
assert pl.nvertices == len(pl.vertices) == 1
assert len(pl.segment_lengths) == pl.nsegments == 0
assert pl.vertices[0, 0] == v[0][0]
assert pl.vertices[0, 1] == v[0][1]
return
def parse(self, line):
if Uni_node.parse(line, self.road_items):
return True
if Multi_node.parse(line, self.road_items):
return True
if Traffic_signal.parse(line, self.road_items):
return True
if Link.parse(line, self.road_items):
return True
if Road_segment.parse(line, self.road_items, self.road_segments):
return True
if Polyline.parse(line, self.road_items):
return True
if Lane.parse(line, self.road_items):
return True
if Crossing.parse(line, self.road_items, self.crossings):
return True
if Connector.parse(line, self.road_items):
return True
#if Circular.parse(line, self.road_items):
# return True
return False
def __init__(self, filename):
self.things = dict()
self.roads = dict()
self.crossings = list()
self.lanes = list()
self.polylines = list()
f = open(filename, 'r')
for line in f:
if Node.check(line, self.things):
continue
elif Section.check(line, self.things, self.roads):
continue
elif Crossing.check(line, self.crossings):
continue
elif Lane.check(line, self.lanes):
continue
elif Turning.check(line, self.things):
continue
elif Polyline.check(line, self.polylines):
continue
self.post_process()
def test_replace_vertex_one_segment(self):
v = [[0, 0], [1, 0]]
pl = Polyline(v)
# Test 1: Move first vertex
v_repl = [-1, 0]
pl.replace_vertex(0, v_repl)
v_expected = [v_repl] + v[1:]
assert len(pl.vertices) == pl.nvertices == len(v_expected)
assert len(pl.segment_lengths) == pl.nsegments == pl.nvertices - 1
for i, vi in enumerate(v_expected):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
if i < pl.nsegments:
assert pl.segment_lengths[i] == approx(
norm(vi[0], vi[1], v_expected[i + 1][0],
v_expected[i + 1][1]))
# Test 2: Move second vertex
v_repl_2 = [2, 0]
pl.replace_vertex(1, v_repl_2)
v_expected = [v_repl, v_repl_2]
assert len(pl.vertices) == pl.nvertices == len(v_expected)
assert len(pl.segment_lengths) == pl.nsegments == pl.nvertices - 1
for i, vi in enumerate(v_expected):
assert pl.vertices[i, 0] == vi[0]
assert pl.vertices[i, 1] == vi[1]
if i < pl.nsegments:
assert pl.segment_lengths[i] == approx(
norm(vi[0], vi[1], v_expected[i + 1][0],
v_expected[i + 1][1]))
return
from circle import Circle
from arc import Arc
from block import Block
from fileformats.render2dxf import Render2DXF
from twod_operations import *
#from toacadscript import element2Script, elements2Script, toFile
if __name__ == "__main__":
p1=Point(1,2,3)
p2=Point(4,5,6)
l1=Line(p1,p2)
print "p1 = " + str(p1)
print "p2 = " + str(p2)
print "l1 = " + str(l1)
print "l1.length() = %f" % l1.length()
pl1=Polyline([Point(0,0),Point(1,1),Point(1,0),Point(0,0)],closed=True)
print "pl1 = " + str(pl1)
print "pl1.length() = %f" % pl1.length()
print "pl1.area() = %f" % pl1.area()
v1 = Vector(1,2,3)
print "v1 = " + str(v1)
v2 = Vector(1,0,0)
print "v2 = " + str(v2)
v3 = Vector(0,1,0)
print "v3 = " + str(v3)
print "v1 dot v2 = " + str(v1.dot(v2))
print "v2 cross v3= " + str(v2.cross(v3))
print "v1 * 3 = " + str(v1 * 3)
print "2 * v1 = " + str(2.0 * v1) #this doesn;t work becuse of the ordering of the arguments to __mul__, that sucks!
print "v1 / 3 = " + str(v1/3)
print "abs(v1) = " + str(abs(v1))
class Grid:
def __init__(self, x_size: int, y_size: int):
if y_size < 4 or x_size < 4:
raise ValueError("Grids must be at least 6x6 in size")
self.x_size = x_size
self.y_size = y_size
self.x_max = 0
self.y_max = 0
self.phrase = ""
self.win = None
self.ineligible_points = []
self.letter_codes = {
"A": "UURDLRD",
"B": "UURDLRDLR",
"C": "UURLDDR",
"D": "URUDDLR",
"E": "UURLDRLDR",
"F": "UURLDRLDR",
"G": "UURLDDRUlrD",
"H": "UUDRUDD",
"I": "UUDD",
"J": "UDRUULRDD",
"K": "E",
"L": "UUDD",
"M": "UURDDUURDD",
"N": "UURDD",
"O": "UURDDLR",
"P": "UURDLDR",
"Q": "RUULDRD",
"R": "UURLDDR",
"S": "RULURLDRD",
"T": "UUlrrlDD",
"U": "UUDDRUUDD",
"V": "UUDDRUUDD",
"W": "UUDDRUUDDRUUDD",
"X": "E",
"Y": "RUUDLUDRD",
"Z": "E",
" ": "R",
}
def verify_drawing(self, x_origin: int, y_origin: int) -> bool:
for point in self.cursor.get_points():
if point.x > self.x_size or point.y > self.y_size:
return False
for x in range(x_origin, self.x_max + 1):
for y in range(y_origin, self.y_max + 1):
for point in self.ineligible_points:
if point.x == x or point.y == y:
return False
return True
def get_max_x(self):
maxX = 0
for point in self.cursor.get_points():
if point.x > maxX:
maxX = point.x
return maxX
def get_max_y(self):
maxY = 0
for point in self.cursor.get_points():
if point.y > maxY:
maxY = point.y
return maxY
def get_max_cords(self):
print(f"Max: [{self.get_max_x()},{self.get_max_y()}]")
return self.get_max_x(), self.get_max_y()
def draw_letter(self, letter: str):
if len(letter) > 1:
raise ValueError("draw_letter only accepts one char at a time")
for command in self.letter_codes[letter]:
if command == "L":
self.cursor.go_left()
elif command == "R":
self.cursor.go_right()
elif command == "U":
self.cursor.go_up()
elif command == "D":
self.cursor.go_down()
if command == "l":
self.cursor.go_halfway_left()
if command == "r":
self.cursor.go_halfway_right()
if command == "u":
self.cursor.go_halfway_up()
if command == "d":
self.cursor.go_halfway_down()
if command == "E":
raise ValueError("Cannot print K, X, Y")
def draw_phrase(self, phrase: str, x: int, y: int):
self.cursor = Polyline(x, y)
self.phrase = phrase.upper()
for letter in self.phrase[:-1]:
self.draw_letter(letter)
if letter is not " ":
self.cursor.go_right()
self.draw_letter(self.phrase[-1])
def render(self, phrase: str):
self.draw_phrase(phrase, 0, 0)
self.x_max, self.y_max = self.get_max_cords()
for x in range(self.x_size - self.x_max + 1):
for y in range(self.y_size - self.y_max + 1):
self.draw_phrase(phrase, x, y)
successful_drawing = self.verify_drawing(x, y)
if successful_drawing:
return
raise ValueError("Drawing cannot fit onto the grid")
def add_ineligible_point(self, x: int, y: int):
self.ineligible_points.append(Point(x, y))
def setup_graphic(self, scale: int):
self.win = GraphWin("Road Art", 4 * scale * len(self.phrase),
7 * scale)
self.win.yUp()
def display_graphic(self, scale: int):
point_list = self.cursor.get_points()
for i in range(len(point_list) - 1):
ln = Line(
Point(point_list[i].x * scale, point_list[i].y * scale),
Point(point_list[i + 1].x * scale,
point_list[i + 1].y * scale))
ln.setWidth(4)
ln.draw(self.win)
def display_eligible_points(self, scale: int):
for x in range(self.x_size):
for y in range(self.y_size):
cir = Circle(Point(x * scale, y * scale), 2)
cir.setOutline('green')
cir.setFill('green')
cir.draw(self.win)
def display_ineligible_points(self, scale: int):
for point in self.ineligible_points:
cir = Circle(Point(point.x * scale, point.y * scale), 8)
cir.setOutline('red')
cir.setFill('red')
cir.draw(self.win)
def render_graphic(self):
self.win.getMouse()
self.win.close()
def __str__(self):
return str(self.cursor)
def __init__(self):
pygame.init()
# Set Width
self.WIDTH = 900
# Set Height
self.HEIGHT = 500
# Set pictures folder path
self.base_path = os.path.dirname(
os.path.abspath(__file__)) + '/pictures/'
# Create screen
self.screen = pygame.display.set_mode((self.WIDTH, self.HEIGHT))
# "Background image" from screen, check draw function from Line class to better understanding
self.layer = pygame.surface.Surface((self.WIDTH, self.HEIGHT))
# Remember pressed keys
self.keyboard = [0] * 255
# Set current tool to nothing
self.current_tool = Tool()
# Set background initial color and tool icons
self.layer.fill((255, 255, 255))
line_icon = pygame.image.load(self.base_path + 'line.png')
line_icon = pygame.transform.scale(line_icon, (50, 50))
line_rect = line_icon.get_rect()
line_rect.move_ip(0, 0)
self.layer.blit(line_icon, line_rect)
curve_icon = pygame.image.load(self.base_path + 'curve.png')
curve_icon = pygame.transform.scale(curve_icon, (50, 50))
curve_rect = curve_icon.get_rect()
curve_rect.move_ip(50, 0)
self.layer.blit(curve_icon, curve_rect)
circle_icon = pygame.image.load(self.base_path + 'circle.png')
circle_icon = pygame.transform.scale(circle_icon, (50, 50))
circle_rect = circle_icon.get_rect()
circle_rect.move_ip(100, 0)
self.layer.blit(circle_icon, circle_rect)
square_icon = pygame.image.load(self.base_path + 'square.png')
square_icon = pygame.transform.scale(square_icon, (50, 50))
square_rect = square_icon.get_rect()
square_rect.move_ip(150, 0)
self.layer.blit(square_icon, square_rect)
polyline_icon = pygame.image.load(self.base_path + 'polyline.png')
polyline_icon = pygame.transform.scale(polyline_icon, (50, 50))
polyline_rect = polyline_icon.get_rect()
polyline_rect.move_ip(200, 0)
self.layer.blit(polyline_icon, polyline_rect)
rectangle_icon = pygame.image.load(self.base_path + 'rectangle.png')
rectangle_icon = pygame.transform.scale(rectangle_icon, (50, 50))
rectangle_rect = rectangle_icon.get_rect()
rectangle_rect.move_ip(250, 0)
self.layer.blit(rectangle_icon, rectangle_rect)
bucket_icon = pygame.image.load(self.base_path + 'bucket.png')
bucket_icon = pygame.transform.scale(bucket_icon, (50, 50))
bucket_rect = bucket_icon.get_rect()
bucket_rect.move_ip(300, 0)
self.layer.blit(bucket_icon, bucket_rect)
self.color_values = [
'#173F5F',
'#20639B',
'#3CAEA3',
'#F6D55C',
'#ED553B',
'#e72020',
'#4de32a',
'#28cae1',
'#f477dc',
'#fbf230',
'#08469E',
'#C2352D',
'#F4F3F4',
'#824C41',
'#D89E6D',
'#007065',
'#FFFFFF',
'#000000',
]
self.color = []
self.color_rects = []
for i in range(ceil(len(self.color_values) / 2)):
self.color.append(pygame.Color(self.color_values[i]))
self.color_rects.append(pygame.Rect(400 + (i * 25), 0, 25, 25))
pygame.draw.rect(self.layer, self.color[i], self.color_rects[i])
for i in range(ceil(len(self.color_values) / 2),
len(self.color_values)):
self.color.append(pygame.Color(self.color_values[i]))
self.color_rects.append(
pygame.Rect(
400 + ((i - (ceil(len(self.color_values) / 2))) * 25), 25,
25, 25))
pygame.draw.rect(self.layer, self.color[i], self.color_rects[i])
self.screen.blit(self.layer, (0, 0))
pygame.display.flip()
self.layer.blit(self.screen, (0, 0))
self.current_color = pygame.Color('#000000') #black
while True:
for event in pygame.event.get():
self.screen.blit(self.layer, (0, 0))
if event.type == QUIT:
pygame.quit()
exit()
color_changed_flag = False
if event.type == MOUSEBUTTONDOWN and event.button == 1:
for i in range(len(self.color_rects)):
if self.Inside(self.color_rects[i]):
print(f"Color {self.color_values[i]} selected")
self.current_color = self.color[i]
self.current_tool.att_color(self.color[i])
color_changed_flag = True
break
if color_changed_flag:
continue
else:
if self.Inside(line_rect):
self.current_tool = Line(self.current_color)
print("Line tool selected")
continue
elif self.Inside(polyline_rect):
self.current_tool = Polyline(self.current_color)
print("Polyline tool selected")
continue
elif self.Inside(rectangle_rect):
self.current_tool = Rectangle(self.current_color)
print("Rectangle tool selected")
continue
elif self.Inside(square_rect):
self.current_tool = Square(self.current_color)
print("Square tool selected")
continue
elif self.Inside(circle_rect):
self.current_tool = Circle(self.current_color)
print("Circle tool selected")
continue
elif self.Inside(curve_rect):
self.current_tool = Curve(self.current_color)
print("Curve tool selected")
continue
elif self.Inside(bucket_rect):
self.current_tool = Fill(self.current_color)
print("Fill tool selected")
continue
self.current_tool.draw(self.screen, self.layer, event,
pygame.mouse.get_pos()[0],
pygame.mouse.get_pos()[1],
self.keyboard)
pygame.display.flip()
[unicycle_v0]
])
stacked_x0 = np.concatenate([car1_x0, car2_x0, unicycle_x0], axis=0)
car1_Ps = [np.zeros((car1._u_dim, dynamics._x_dim))] * HORIZON_STEPS
car2_Ps = [np.zeros((car2._u_dim, dynamics._x_dim))] * HORIZON_STEPS
unicycle_Ps = [np.zeros((unicycle._u_dim, dynamics._x_dim))] * HORIZON_STEPS
car1_alphas = [np.zeros((car1._u_dim, 1))] * HORIZON_STEPS
car2_alphas = [np.zeros((car2._u_dim, 1))] * HORIZON_STEPS
unicycle_alphas = [np.zeros((unicycle._u_dim, 1))] * HORIZON_STEPS
# Create environment.
car1_position_indices_in_product_state = (0, 1)
car1_polyline = Polyline([Point(6.0, -100.0), Point(6.0, 100.0)])
car1_polyline_boundary_cost = SemiquadraticPolylineCost(
car1_polyline, 1.0, car1_position_indices_in_product_state,
"car1_polyline_boundary")
car1_polyline_cost = QuadraticPolylineCost(
car1_polyline, car1_position_indices_in_product_state, "car1_polyline")
car1_goal = Point(6.0, 30.0)
car1_goal_cost = ProximityCost(
car1_position_indices_in_product_state, car1_goal, np.inf, "car1_goal")
car2_position_indices_in_product_state = (5, 6)
car2_polyline = Polyline([Point(2.0, 100.0),
Point(2.0, 18.0),
Point(2.5, 15.0),
Point(3.0, 14.0),
def test_segment_length_gradient_last(self):
test_eps = 1e-2 # Max allowed deviation, chose suitable for num grad
idx = 1 # Movable vertex index, here second vertex is tested
# Test dL/dx
pl = Polyline([[0, 0], [1, 0]])
x_pos = np.linspace(-2, 2, 501)
y_pos = [-1., -0.5, 0.25, 0.5, 1.] # Don't overlap vertices
for yi in y_pos:
seg_lens, seg_lens_grad_x = [], []
for xi in x_pos:
pl.replace_vertex(idx, [xi, yi])
sl = pl.get_segment_length(0)
sl_grad = pl.get_segment_length_grad(0, which="last")
seg_lens.append(sl)
seg_lens_grad_x.append(sl_grad[0])
# Test analytical against numerical gradients
num_grad_x = np.gradient(seg_lens, x_pos)
assert np.allclose(num_grad_x, seg_lens_grad_x, atol=test_eps)
# Test dL/dy
pl = Polyline([[0, 0], [1, 0]])
x_pos = [-1.5, -1., -0.5, 0.5, 1.5] # Don't overlap vertices
y_pos = np.linspace(-2, 2, 501)
for xi in x_pos:
seg_lens, seg_lens_grad_y = [], []
for yi in y_pos:
pl.replace_vertex(idx, [xi, yi])
sl = pl.get_segment_length(0)
sl_grad = pl.get_segment_length_grad(0, which="last")
seg_lens.append(sl)
seg_lens_grad_y.append(sl_grad[1])
# Test analytical against numerical gradients
num_grad_y = np.gradient(seg_lens, y_pos)
assert np.allclose(num_grad_y, seg_lens_grad_y, atol=test_eps)
return
