def square(cls, size):
'''Creates square FP. size [int] square side length'''
obj = cls.__new__(cls)
obj.size = size
points = []
center = Point(0, 0)
for x in range(-size // 2, (size // 2)):
for y in range(-size // 2, (size // 2)):
points += [Point(x + 1, y + 1)]
obj.points = points
obj.is_square = True
return obj
def __init__(self, sess, coords, ftprint, \
heal_pts, owner, armor, weapon, speed):
if speed is None:
raise ValueError('Unit speed was not specified')
super().__init__(sess, coords, ftprint, heal_pts, owner, armor, weapon)
self.speed = speed
self.dest = self.coords
self.node = Point(-1, -1)
self.direction = Point(0, 0)
self.nodes = []
self.otype = 'U'
self.add_cmd('stop', self.session.cmds.stop, (1,0))
self.add_cmd('move', self.session.cmds.move, (2,0))
def test_point_and_vector_addition(self):
p = Point(3, 5)
v = Vector(0, 1)
p2 = p + v
self.assertEqual(3, p2.x)
self.assertEqual(6, p2.y)
def export_grid(self, filename):
print('export_grid')
print(self.grid.x_range)
print(self.grid.y_range)
for cell in self.grid.cells:
print('cell: %s - color:%s' % (cell, self.grid.cells[cell].color))
x_min = self.grid.x_range[0] - 2
x_max = self.grid.x_range[1] + 2
y_min = self.grid.y_range[0] - 2
y_max = self.grid.y_range[1] + 2
with open(filename, "w") as simulation_file:
simulation_file.write('x range: [%d,%d]\n' % (x_min, x_max))
simulation_file.write('y range: [%d,%d]\n' % (y_min, y_max))
simulation_file.write('\n')
y = y_max
while y >= y_min:
simulation_file.write('|')
x = x_min
while x <= x_max:
cell = self.grid.cells.get(Point(x, y))
if cell is None or cell.color == Color.WHITE:
simulation_file.write('W|')
elif cell.color == Color.BLACK:
simulation_file.write('B|')
x = x + 1
y = y - 1
simulation_file.write('\n')
class Simulator:
ORIGIN = Point(0, 0)
def __init__(self):
self.robot = Robot(Simulator.ORIGIN, WindRose.NORTH_INDEX)
self.grid = Grid()
def run(self, steps):
step = 0
if steps < 0:
raise Exception('number of steps must be positive(%d)' % steps)
while step < steps:
print('Step %d' % step)
original_cell = self.robot.position
cell = self.grid.add_cell(original_cell)
if cell.color == Color.WHITE:
self.robot.clockwise_rotate()
elif cell.color == Color.BLACK:
self.robot.anti_clockwise_rotate()
cell.flip()
self.robot.move()
step = step + 1
def export_grid(self, filename):
print('export_grid')
print(self.grid.x_range)
print(self.grid.y_range)
for cell in self.grid.cells:
print('cell: %s - color:%s' % (cell, self.grid.cells[cell].color))
x_min = self.grid.x_range[0] - 2
x_max = self.grid.x_range[1] + 2
y_min = self.grid.y_range[0] - 2
y_max = self.grid.y_range[1] + 2
with open(filename, "w") as simulation_file:
simulation_file.write('x range: [%d,%d]\n' % (x_min, x_max))
simulation_file.write('y range: [%d,%d]\n' % (y_min, y_max))
simulation_file.write('\n')
y = y_max
while y >= y_min:
simulation_file.write('|')
x = x_min
while x <= x_max:
cell = self.grid.cells.get(Point(x, y))
if cell is None or cell.color == Color.WHITE:
simulation_file.write('W|')
elif cell.color == Color.BLACK:
simulation_file.write('B|')
x = x + 1
y = y - 1
simulation_file.write('\n')
def project_to_image(self, image, camera):
# TODO: project circle to image
### your code starts ###
w_center = self.center.to_vector()
x0 = int(w_center[0][0])
y0 = int(w_center[1][0])
z0 = int(w_center[2][0])
p0 = np.array([x0, y0, z0])
p1 = np.array([x0 + self.radius, y0, z0])
p2 = np.array([x0, y0 + self.radius,
z0]) # Need to trans P1, P2(type:ndarray) to Point
print('Center Point of the circle:' + str(p0))
print('One of the perpendicular radius cross circle-rim point is:' +
str(p1))
print('Another perpendicular radius cross circle-rim point is:' +
str(p2))
P0 = Point(x0, y0, z0)
P1 = Point(x0 + self.radius, y0, z0)
P2 = Point(x0, y0 + self.radius, z0)
r0 = camera.project(P0)
r1 = camera.project(P1)
r2 = camera.project(P2)
R0 = r0.to_vector()
R00 = np.array([R0[0][0][0], R0[1][0][0], R0[2][0]])
R1 = r1.to_vector()
R11 = np.array([R1[0][0][0], R1[1][0][0], R1[2][0]])
#R1 = np.array([r1.to_vector()[0][0][0], r1.to_vector()[1][0][0]], r1.to_vector()[2][0]])
R2 = r2.to_vector()
#R2 = np.array([r2.to_vector()[0][0][0], r2.to_vector()[1][0][0]], r2.to_vector()[2][0]])
R22 = np.array([R2[0][0][0], R2[1][0][0], R2[2][0]])
print('Center Point of the projected ellipse:' + str(R00))
print(
'One of the perpendicular radius cross ellipse-rim point after projection is:'
+ str(R11))
print(
'Another perpendicular radius cross ellipse-rim point after projection is:'
+ str(R22))
e_center = (int(R00[0]), int(R00[1]))
d1 = int(np.linalg.norm(R11 - R00))
d2 = int(np.linalg.norm(R22 - R00))
image = cv.ellipse(image, e_center, (d1, d2), 0, 0, 360, self.color, 5)
### your code ends ###
return image
def update(self, tick):
super().update(tick)
points = 0
while self.coords != self.dest and points < self.speed:
prev = self.coords.copy()
self.session.board.release_fp(self)
self.session.tell_tell_dirty()
if self._get_direction(self.coords, self.node) != self.direction:
self.node = self.nodes[0]
self.nodes = self.nodes[1:]
self.direction = self._get_direction(self.coords, self.node)
self.direction_cost = self._get_dir_cost(self.direction)
points += self.direction_cost
self.coords = self.coords + self.direction
self.coords.round(2)
if not self.session.board.apply_fp(self):
self.coords = prev
self.session.board.apply_fp(self)
self.dest = self.coords.copy()
self.node = Point(-1, -1)
self.direction = Point(0, 0)
self.nodes = []
def load_lane_markings():
xs = [float(i) for i in range(100)]
def left_y(x):
return 1.5 + 0.025 * x + 0.001 * x * x + 0.1 * np.sin(x * 0.1) + random.random() * 0.5
def right_y(x):
return -1.5 + 0.01 * x + 0.001 * x * x + 0.1 * np.sin(x * 0.2) + random.random() * 0.5
left_ys = [left_y(x) for x in xs]
right_ys = [right_y(x) for x in xs]
raw_points = [Point(x, y, 0.0) for x, y in zip(xs + xs, left_ys + right_ys)]
first_left = [Point(x, y, 0.0) for x, y in zip(xs[:60], left_ys[:60])]
first_right = [Point(x, y, 0.0) for x, y in zip(xs[:60], right_ys[:60])]
first_left_lane_marking = LaneMarking(first_left)
first_right_lane_marking = LaneMarking(first_right)
trans_x = 50.0
trans_y = 0.5 * (left_y(trans_x) + right_y(trans_x))
yaw = 1.46
pose = Transform(Rotation.from_rpy(0.0, 0.0, yaw), Translation(trans_x, trans_y, 0.0))
pose_inv = np.linalg.inv(pose.homogeneous)
second_left = []
second_right = []
for x, left_y, right_y in zip(xs[40:], left_ys[40:], right_ys[40:]):
left_p = np.array([x, left_y, 0.0, 1.0])
left_p = left_p.reshape((4, 1))
right_p = np.array([x, right_y, 0.0, 1.0])
right_p = right_p.reshape((4, 1))
left_p_inv = np.dot(pose_inv, left_p)
right_p_inv = np.dot(pose_inv, right_p)
second_left.append(Point(left_p_inv[0, 0], left_p_inv[1, 0] + 0.5 * random.random(), 0.0))
second_right.append(Point(right_p_inv[0, 0], right_p_inv[1, 0] + 0.5 * random.random(), 0.0))
second_left_lane_marking = LaneMarking(second_left, pose=pose)
second_right_lane_marking = LaneMarking(second_right, pose=pose)
return (first_left_lane_marking, first_right_lane_marking), (second_left_lane_marking, second_right_lane_marking), raw_points
def sub_occupy(self, obj):
if not self.crossable: return False
if self.occupied == 2: return False
size = obj.footprint.size / 2 # Radius
for x in range(self.size):
for y in range(self.size):
sx, sy = self.coords.get()
coords = Point(sx+(x/self.size), sy+(y/self.size))
v = Vector.from_abs(obj.coords, coords)
if v.magnitude > size:
continue
if self.subcells[y][x] is not None:
return False
self.subcells[y][x] = obj
self.occupied = 1
return True
def load_png(self, filename):
defines = {k:tuple(v) for k,v in self.CLRS.board_file_defines.items()}
pillow = Image.open(filename)
self.size = pillow.size
width, height = pillow.size
pixels = pillow.load()
self.cells = [[None for x in range(width)] for y in range(height)]
var_count = self.CORE.variant_count
sub_per_cell = self.CORE.sub_per_cell
for y in range(height):
for x in range(width):
point = Point(x, y)
try: key = self.find_key(defines, pixels[x, y])
except KeyError:
Log.info('Invalid color {} in map "{}"' + \
' at position {}'.format( pixels[x, y],
path.split('/')[-1], (x,y)))
raise
if key == 'start_pos': self.starts += [point]
elif 'norm' in key or 'rich' in key:
self.toplace+=[(point, key)]
crossable = key != 'not_crsbl'
cell = Cell(point, crossable, sub_per_cell, var_count)
self.cells[y][x] = cell
print(
'Another perpendicular radius cross ellipse-rim point after projection is:'
+ str(R22))
e_center = (int(R00[0]), int(R00[1]))
d1 = int(np.linalg.norm(R11 - R00))
d2 = int(np.linalg.norm(R22 - R00))
image = cv.ellipse(image, e_center, (d1, d2), 0, 0, 360, self.color, 5)
### your code ends ###
return image
if __name__ == "__main__":
camera = create_test_camera_model()
line = ColoredLine(Point(-1.0, -0.5, 1.5), Point(-0.5, -1.0, 3.5))
points = [
Point(-0.5, 0.0, 2.0),
Point(-0.5, 0.0, 4.0),
Point(0.5, 0.5, 4.0),
Point(0.5, 0.5, 2.0)
]
polygon = ColoredPolygon(points)
circle = Circle(Point(0.0, 0.0, 0.0), 0.5)
image = np.ones((768, 1024, 3)) * 255
image = polygon.project_to_image(image, camera)
image = line.project_to_image(image, camera)
image = circle.project_to_image(image, camera)
def _get_direction(self, orig, dest):
dx = (1 if orig.x < dest.x else -1) if orig.x != dest.x else 0
dy = (1 if orig.y < dest.y else -1) if orig.y != dest.y else 0
sub_per_cell = self.session.request_subpercell()
return Point(dx/sub_per_cell, dy/sub_per_cell)
def request_path(self):
self.nodes = self.session.board.request_path( \
self.footprint, self.coords, self.dest)
self.node = Point(-1, -1)
self.direction = Point(0, 0)
def _stop_inst(session, actor):
actor.dest = actor.coords.copy()
actor.node = Point(-1, -1)
actor.nodes = []
actor.direction = Point(0, 0)
def _move_inst(session, actor, coords):
actor.dest = Point(*coords)
actor.request_path()
def _train_soldier_deld(session, actor, coords):
obj = o.Soldier(session, Point(*coords), actor.owner)
session.add_object(obj)
def request_path(self, fp, orig, dest):
orig.to_ints()
dest.to_ints()
return [Point(*pt) for pt in self.finder.find(fp, orig, dest)]
def test_two_point_addition(self):
p1 = Point(3, 5)
p2 = Point(0, 1)
with self.assertRaises(TypeError) as context:
p3 = p1 + p2
def main():
"""Main function.
After providing a sample subdivision, its trapezoidal map and search structure are built.
Then, points can be queried to find which faces contain them.
"""
example = 2 # Number of the example
# Create a sample subdivision.
print("\n\n*** INITIALIZATION ***")
segments = set()
if example == 1:
p1 = Point(2, 4)
q1 = Point(9, 6)
p2 = Point(6, 3)
q2 = Point(12, 2)
s1 = Segment(p1, q1)
s2 = Segment(p2, q2)
segments = {s1, s2}
elif example == 2:
p1 = Point(10, 8)
p2 = Point(2, 4)
p3 = Point(6, 2)
p4 = Point(20, 4)
p5 = Point(12, 10)
p6 = Point(16, 6)
s1 = Segment(p1, p2)
s2 = Segment(p2, p3)
s3 = Segment(p3, p4)
s4 = Segment(p4, p5)
s5 = Segment(p2, p6)
segments = {s1, s2, s3, s4, s5}
S = Subdivision(segments)
# Build the trapezoidal map and the search structure.
print("\n\n*** CONSTRUCTION ***")
S.trapezoidal_map()
# Query a sample point.
print("\n\n*** QUERY ***")
query_point = Point(4, 2)
face = S.T.D.query(query_point)
print("\nPoint " + str(query_point) + " is located here:\n" + str(face))
from src.geometry import Point, Segment, Trapezoid from src.nodes import XNode, YNode, LeafNode # ---GEOMETRY---- # Points p1 = Point(1, 3) q1 = Point(5, 4) p2 = Point(3, 2) q2 = Point(6, 1) ll = Point(0, 0) lr = Point(7, 0) ul = Point(0, 6) ur = Point(7, 6) # Segments s1 = Segment(p1, q1) s2 = Segment(p2, q2) ls = Segment(ll, lr) us = Segment(ul, ur) # Trapezoids A = Trapezoid(us, ls, ll, p1) B = Trapezoid(us, s1, p1, q1) C = Trapezoid(s1, ls, p1, p2) D = Trapezoid(s1, s2, p2, q1) E = Trapezoid(us, s2, q1, q2) F = Trapezoid(s2, ls, p2, q2) G = Trapezoid(us, ls, q2, lr) A.set_neighbors(None, None, B, C) B.set_neighbors(A, None, E, None)
def get_point(self, s):
s_vector = np.array([1.0, s, s * s, s * s * s])
x = np.dot(s_vector, self.coeff_x)
y = np.dot(s_vector, self.coeff_y)
return Point(x, y, 0.0)