def __init__(self,
pos=Point2D(0, 0),
angle=0,
velocity=0,
maxVelocity=5,
bodyWidth=30,
bodyLength=60,
headlightsOn=False,
headlightPower=100):
self.pos = pos #type: Point
self.velocity = velocity #type: float
self.maxVelocity = maxVelocity
self.angle = angle #type: float
self.headlightsOn = headlightsOn #type: bool
self.headlightPower = headlightPower #type: int
self.height = bodyWidth #type: int
self.width = bodyLength #type: int
self.__body = Rectangle(bodyLength, bodyWidth, pos, color=(1, 0, 0))
self.__headlight1 = Sector(Point2D(pos.x, pos.y),
angle,
inner=40,
color=(1, 1, 0.5))
self.__headlight1.hide()
self.__headlight2 = Sector(Point2D(pos.x, pos.y),
angle,
inner=40,
color=(1, 1, 0.5))
self.__headlight2.hide()
def is_catch(self, runner: MazePosition, catcher: MazePosition) -> bool:
"""Check if the catcher catches the runner
"""
# Neither runner nor catcher is in the maze, return False.
if runner.position_detail.x < 0 or catcher.position_detail.x < 0:
return False
if Point2D.distance(runner.position_detail,
catcher.position_detail) < 12.0:
if catcher.position == runner.position:
return True
# Use the position of the runner as the reference point
connection_bit = {
Point2D(0, 1): GameCore.SIDE_UP,
Point2D(1, 0): GameCore.SIDE_RIGHT,
Point2D(0, -1): GameCore.SIDE_DOWN,
Point2D(-1, 0): GameCore.SIDE_LEFT
}.get(catcher.position - runner.position, 0)
# If the block where the runner at and the block where the cather at
# is connected, return True
if self._maze_map[runner.position.y][
runner.position.x] & connection_bit != 0:
return True
return False
def run(self):
angle = Point3D(30.0, 30.0, 0.0)
delta = Point3D(0, 0, 0)
friction_factor = 0.98
init_pos = None
dragging = False
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
dragging = True
init_pos = Point2D(*event.pos)
elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
dragging = False
elif event.type == pygame.MOUSEMOTION and dragging:
curr_pos = Point2D(*event.pos)
delta.y, delta.x = (init_pos -
curr_pos) * 2 * math.pi / self.width
angle += delta
cube = Cube(size=100,
angle_x=angle.x,
angle_y=angle.y,
angle_z=angle.z)
self.__draw_cube(cube)
if not dragging:
delta *= friction_factor
angle += delta
pygame.display.flip()
def _getVec2VecDistancePointVec(vec1, vec2, isMin):
"Get least/farthest distance"
spoints = vec1.pointList # starting points
nspoint = Point2D([0, 0])
nepoint = Point2D([0, 1])
svec = None
if isMin is True:
distance = float('inf')
else:
distance = float('-inf')
for sp in spoints:
#
distancePoint = vec2.getPoint2VecDistancePoint(point=sp,
isMin=isMin)
epoint = distancePoint[0]
dist = distancePoint[1]
if isMin is True:
checkval = dist <= distance
else:
checkval = dist > distance
if checkval:
distance = dist
nspoint = sp
nepoint = epoint
svec = LocatedVector2D(nspoint, nepoint)
#
svec.setVecProperties()
return nspoint, nepoint, svec, distance
def recognize_maze(self, scale_x: int, scale_y: int, wall_height: float, \
upper_corner: list, lower_corner: list):
"""Generate the transform matries and set them to all MazePositionFinder
@param scale_x The x scale of the maze
@param scale_y The y scale of the maze
@param wall_height The height of the maze wall
@param upper_corner A list storing point2D of 4 corners on the upper plane
@param lower_corner A list storing point2D of 4 corners on the lower plane
"""
maze_scale = Point2D(scale_x, scale_y)
maze_scale_detail = Point2D(128, 128)
upper_transform_mat = self._generate_transform_matrix(
upper_corner, maze_scale)
lower_transform_mat = self._generate_transform_matrix(
lower_corner, maze_scale)
upper_transform_mat_detail = \
self._generate_transform_matrix(upper_corner, maze_scale_detail)
lower_transform_mat_detail = \
self._generate_transform_matrix(lower_corner, maze_scale_detail)
for maze_pos_finder in self._maze_pos_finders.values():
maze_pos_finder.set_transform_matrix(upper_transform_mat, lower_transform_mat, \
upper_transform_mat_detail, lower_transform_mat_detail)
maze_pos_finder.set_wall_height(wall_height)
def __init__(self, color_bgr, LED_height: float):
"""Constructor
@param color_bgr Specify the color of the LED on the maze car
@param LED_height Specify the height of LED on the maze car
"""
self.color_bgr = color_bgr
self.LED_height = LED_height
self.position = Point2D(-1, -1)
self.position_detail = Point2D(-1, -1) # Always in 128 x 128 scale
self._missing_counter = 0 # It will not be copied.
def update(dummy):
global point
if point.dist(car.pos) < 300:
x = randrange(window.get_size()[0])
y = randrange(window.get_size()[1])
newPoint = Point2D(x,y)
while newPoint.dist(point) > 300:
x = randrange(window.get_size()[0])
y = randrange(window.get_size()[1])
newPoint = Point2D(x,y)
point = newPoint
car.turnToward(point)
car.drive()
def _find_target_color(target_frame_hsv, target_color_hsv):
"""Find the position of the specified color in the given frame
@param target_frame_hsv The source frame in HSV domain
@param target_color_hsv The color in the HSV domain to be found
in the target_frame_hsv
@return A list of positions in pixel where the target color is at
It is possible that returning an empty list
"""
lower_bound, upper_bound = _get_detect_range(target_color_hsv)
# Only colors in defined range will be passed
filtered_frame = cv2.inRange(target_frame_hsv, lower_bound,
upper_bound)
# Erode and dilate the filtered result with 3 x 3 kernal
# to eliminate the noise
kernal = np.ones((3, 3), dtype=np.uint8)
filtered_frame = cv2.erode(filtered_frame, kernal, iterations=1)
filtered_frame = cv2.dilate(filtered_frame, kernal, iterations=1)
filtered_frame = cv2.GaussianBlur(filtered_frame, (5, 5), 0)
# Find contours in the final filtered frame
contours = cv2.findContours(filtered_frame, \
cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
contours = contours[0] if imutils.is_cv2() else contours[1]
# Find center point of each contour
centres = []
for i in range(len(contours)):
moments = cv2.moments(contours[i])
centres.append(Point2D(int(moments['m10']/moments['m00']), \
int(moments['m01']/moments['m00'])))
return centres
def _get_pos(pos_in_frame, ratio_to_wall_height, \
upper_transform_mat, lower_transform_mat) -> Point2D:
""" Transform the pixel position to the maze coordinate
First, transfrom the pixel position by MazePositionFinder._upper_transform_mat
for upper plane (wall level), and MazePositionFinder._lower_transform_mat
for lower plane (groud level). It will generate two coordinates,
pos_at_upper_plane and pos_at_lower_plane.
Then, get the maze coordinate by interploting these two coordinates.
The formula is:
pos_at_lower_plane + (pos_at_upper_plane - pos_at_lower_plane)
* ratio_to_wall_height.
@param pos_in_frame Specify the position found in the video stream
@param ratio_to_wall_height Specify the ratio of the LED height to the wall
height
@param upper_transform_mat Specify the transform matrix of the upper plane
@param lower_transform_mat Specify the transform matrix of the lower plane
@return A Point2D object that stores the maze position in integer
"""
pos = np.array([[[pos_in_frame.x, pos_in_frame.y]]],
dtype=np.float32)
pos_at_upper_plane = cv2.perspectiveTransform(
pos, upper_transform_mat)
pos_at_lower_plane = cv2.perspectiveTransform(
pos, lower_transform_mat)
pos_in_maze = pos_at_lower_plane + \
(pos_at_upper_plane - pos_at_lower_plane) * ratio_to_wall_height
return Point2D(int(round(pos_in_maze[0][0][0] - 0.5)), \
int(round(pos_in_maze[0][0][1] - 0.5)))
def update(dummy):
for ball1 in balls:
ball1.tryUpdate()
for ball2 in balls:
if ball1 != ball2:
ball1.collide(ball2)
ball1.bounce(Point2D(window.get_size()[0], window.get_size()[1]))
def resize_curve(self):
###
#
###
start_p, size = self.get_dimentions()
x_factor = 1
y_factor = 1
stretch_factor = 1
#mensuring factors
if size.x > self.window_size.x:
x_factor = self.window_size.x / size.x
if size.y > self.window_size.y:
y_factor = self.window_size.y / size.y
if x_factor != y_factor:
stretch_factor = min([x_factor, y_factor])
#appling transformations
for n in range(0, len(self._lop)):
self._lop[n] = self._lop[n] * stretch_factor
#calculating translation vector
start_p, size = self.get_dimentions()
translation = Point2D(0, 0) - start_p
for n in range(0, len(self._lop)):
self._lop[n] = self._lop[n] + translation
def add_point(self, x, y):
if isinstance(x, numbers.Number) and isinstance(y, numbers.Number):
self._lop += [Point2D(x, y)]
self._lopS += 1
if not self._bezier:
self.calc_degree()
else:
raise Exception("need Curve.add_point(number, number), got (" +
str(type(x)) + ", " + str(type(y)) + ")")
def _load_config(self):
"""Load the configruation
If the specified file dosen't exist, it will invoke
ConfigManager.save_config() to create and initialize the configuration,
and save to the file.
"""
if not os.path.isfile(self._config_file_path):
self._logger.debug("Config file not found. Create a new one.")
self.save_config()
return
self._logger.debug("Loading config file {0}".format(
self._config_file_path))
config_tree = ET.parse(self._config_file_path)
config_root = config_tree.getroot()
# Maze configuration
self.maze_config["corner_plane_upper"].clear()
corner_upper = config_root.find("./maze/corner[@plane='upper']")
for point in list(corner_upper):
x = int(point.attrib["x"])
y = int(point.attrib["y"])
self.maze_config["corner_plane_upper"].append(Point2D(x, y))
self.maze_config["corner_plane_lower"].clear()
corner_lower = config_root.find("./maze/corner[@plane='lower']")
for point in list(corner_lower):
x = int(point.attrib["x"])
y = int(point.attrib["y"])
self.maze_config["corner_plane_lower"].append(Point2D(x, y))
maze_scale = config_root.find("./maze/scale/Point2D")
self.maze_config["scale"] = Point2D( \
int(maze_scale.attrib["x"]), int(maze_scale.attrib["y"]))
maze_wall_height = config_root.find("./maze/wall_height")
self.maze_config["wall_height"] = float(maze_wall_height.text)
# Server configuration
server_ip = config_root.find("./server/ip")
self.server_config["ip"] = server_ip.text
server_port = config_root.find("./server/port")
self.server_config["port"] = int(server_port.text)
self._logger.debug("Config file is loaded.")
def corners(self):
points = []
points += [
self.pos + Point2D(self.width * cos(self.angle),
self.height * sin(self.angle))
]
points += [
self.pos + Point2D(self.width * cos(self.angle),
-self.height * sin(self.angle))
]
points += [
self.pos + Point2D(-self.width * cos(self.angle),
self.height * sin(self.angle))
]
points += [
self.pos + Point2D(-self.width * cos(self.angle),
-self.height * sin(self.angle))
]
return points
def generate():
global width, height
global balls
for num in range(5):
ball = Ball(Point2D(0, 0), 0, Vector2D(0, 0), (1, 1, 1))
colliding = True
while (colliding):
radius = randint(1, 10) * 10
pos = Point2D.randPoint((radius, width - radius),
(radius, height - radius))
vel = Vector2D(uniform(-3, 3), uniform(-3, 3))
ball = Ball(pos, radius, vel, (random(), random(), random()))
colliding = False
for other in balls:
if (ball.colliding(other)):
colliding = True
break
balls += [ball]
def __init__(self, point1, point2, color=(0, 0, 0), drawable=True):
super().__init__(Point2D(0, 0),
0,
drawable,
color,
draw_method=GL_LINES)
self.point1 = point1 #type: Point2D
self.point2 = point2 #type: Point2D
if drawable:
self.vertices = [0, 0, point1.x - point2.x, point1.x - point2.y]
self.vlist = pyglet.graphics.vertex_list(2, ('v2f', self.vertices))
def _add_point(self, point):
if type(point) is Point2D:
self._lop += [point]
elif type(point) is Vector2D:
self._lop += [Point2D(point.x, point.y)
] #equivalent to point + Point2D(0, 0)
else:
raise Exception("need Curve._add_point(Point2D), got (" +
str(type(point)) + ")")
self._lopS += 1
if not self._bezier:
self.calc_degree()
def __init__(self, config_file_path):
"""Constructor and load the configuration file
@param _config_file_path Specify the file path of the configuration file
"""
self._logger = logging.getLogger(self.__class__.__name__)
self._config_file_path = config_file_path
self.maze_config = {
"corner_plane_upper": \
[Point2D(-1, -1), Point2D(-1, -1), Point2D(-1, -1), Point2D(-1, -1)],
"corner_plane_lower": \
[Point2D(-1, -1), Point2D(-1, -1), Point2D(-1, -1), Point2D(-1, -1)],
"scale": Point2D(-1, -1),
"wall_height": -1
}
self.server_config = {"ip": "127.0.0.1", "port": 5000}
self._load_config()
def __init__(self, win_x, win_y):
super()
self.color = "#ff0000"
self.show_points = False
self.show_lines = False
self.window_size = Point2D(win_x, win_y)
#deleting useless attibuts
self._degree = 3
del self._degree
self._master = False
del self._master
self._delc = []
del self._delc
self._bcurve = []
del self._bcurve
def __init__(self, *points):
# Point sequence
if all(isinstance(p, Point2D) for p in points):
if len(points) > 2:
raise InvalidGeometry("Not enough points")
self.points = list(points)
# Iterable
elif isinstance(points[0], list) or \
isinstance(points[0], tuple):
if len(points[0]) > 2:
raise InvalidGeometry("Not enough points")
self.points = list(points[0])
else:
raise InvalidGeometry("Provide only points or one point iterable")
# Close polygon
first = self.points[0]
end = Point2D(first.x, first.y)
self.points.append(end)
def calc_bcurce_curvature(self, mcurve):
###
# x'(t)y''(t) - y'(t)x''(t)
# _________________________ curvature
# (x'(t)² y'(t)²)^(3/2)
###
self.is_calced = True
#erase possible old content
self._lop = []
self.__prime = []
self.__second = []
self.__bcurve = []
#update bcurve
self.__bcurve = mcurve._bcurve._lop
#derivatives
if self.__bcurve is not None:
#prime derivative
for n in range(0, len(self.__bcurve) - 1):
self.__prime += [
(self.__bcurve[n + 1] - self.__bcurve[n]).make_point()
]
#second derivative
for n in range(0, len(self.__prime) - 1):
self.__second += [
(self.__prime[n + 1] - self.__prime[n]).make_point()
]
#curvature
for n in range(0, len(self.__second)):
self._lop += [(self.__prime[n].x * self.__second[n].y -
self.__prime[n].y * self.__second[n].x) /
(self.__prime[n].module())**3]
self._lop[n] = Point2D(self.__bcurve[n].x, 1 / self._lop[n])
#fiting curve on screen
self.resize_curve()
else:
print("There's no bezier curve to calculate its curvature")
def get_dimentions(self):
###
# get dimentions
###
max_x = self._lop[0].x
max_y = self._lop[0].y
min_x = self._lop[0].x
min_y = self._lop[0].y
for n in range(1, len(self._lop)):
if self._lop[n].x > max_x:
max_x = self._lop[n].x
elif self._lop[n].x < min_x:
min_x = self._lop[n].x
if self._lop[n].y > max_y:
max_y = self._lop[n].y
elif self._lop[n].y < min_y:
min_y = self._lop[n].y
#since the screen goes right and down as (++) quadrant, min_p is the start draw point
return Point2D(min_x, min_y), Vector2D(max_x - min_x, max_y - min_y)
def _getPoint2VecDistancePoint(aVec, point, isMin: bool):
"Get closest/farthest point on vec with distance to the given point"
points = aVec.pointList
npoint = Point2D([0, 0]) # point at origin
if isMin is True:
retval = float('inf')
else:
retval = float('-inf')
for p in points:
# assert p.ndim == point.ndim
vec = LocatedVector(p, point)
vecnorm = vec.getNorm()
checkval = bool
if isMin is True:
checkval = vecnorm < retval
else:
checkval = vecnorm > retval
if checkval:
retval = vecnorm
npoint = p
return npoint, retval
# 06_02-repr() from point import Point2D p = Point2D(3, 4) repr(p) p p2 = Point2D(x=3, y=4) p2 import pdb p = Point2D(3, 5) pdb.set_trace() print(p)
# 06_01-Two String Representations from point import Point2D p = Point2D(x=42, y=69) str(p) repr(p)
return "POLYGON({})".format(geom)
def __eq__(self, other):
for sp, op in zip(self.points, other.points):
if sp != op:
return False
return True
def __ne__(self, other):
return not self.__eq__(other)
def area(self):
area, pts = 0.0, self.points
for i in range(len(pts) - 1):
area += pts[i].x * pts[i + 1].y - pts[i + 1].x * pts[i].y
return abs(area) / 2.0
def is_convex(self):
pass
def is_concave(self):
return not self.is_convex()
p = Point2D(1, 1)
q = Point2D(2, 2)
r = Point2D(1, 2)
l = (p, q, r)
pol1 = Polygon([])
print(pol1.area())
# 06_03-str()
from point import Point2D
p = Point2D(123, 456)
str(p)
print('The Circle is centered at {}'.format(p))
print('The Circle is centered at {}'.format(repr(p)))
# 06_04-When Are the Representations Used
from point import Point2D
str(Point2D(3, 5))
repr(Point2D(3, 5))
print(Point2D(x=3, y=5))
#comment __str__
from point import Point2D
p = Point2D(45, 54)
str(p)
#comment __repr__
from point import Point2D
p = Point2D(45, 54)
# '<point.Point2D object at 0x7f36718c6b38>'
repr(p)
# uncomment everything
from point import Point2D
l = [Point2D(i, i * 2) for i in range(3)]
str(l)
repr(l)
d = {i: Point2D(i, i * 2) for i in range(3)}
str(d)
repr(d)
def intenzity(self, azimut, elevation):
"""
Returns light intenzity for input azimut and elevation. Azimut is not
limited. Elevation use bound value if lowest or highes value exceeded.
:param azimut: [deg] accepts all azimuts (not limited)
:param elevation: [deg] accepts <-180, 180>
:returns [cd]
"""
# range for elevation
assert (-180 <= elevation <= 180)
# azimut not limited, just convert into basic interval <0, 360)
if elevation < 0:
elevation = -elevation
azimut -= 180
azimut = azimut % 360
def angle_diff(first, second):
""" Angle between two azimuts """
diff = (second - first) % 360
if diff > 180:
diff = 360 - diff
return diff
def get_azimut_index_pair(azimut_axis: np.array,
azimut) -> Tuple[float, float]:
"""
:param azimut_axis: sorted list of values covering full circle
"""
penalty_axis = np.abs(azimut_axis - azimut) % 360
min_index = np.argmin(penalty_axis)
cindex = lambda x: x % len(azimut_axis)
if penalty_axis[cindex(min_index -
1)] < penalty_axis[cindex(min_index + 1)]:
return (min_index, cindex(min_index - 1))
else:
return (min_index, cindex(min_index + 1))
def get_elevation_index_pair(elevation_axis: np.array,
elevation) -> Tuple[int, int]:
"""
"""
penalty_axis = np.abs(elevation_axis - elevation)
min_index = np.argmin(penalty_axis)
if min_index == 0:
return (0, None)
elif min_index == len(elevation_axis) - 1:
return (min_index, None)
elif penalty_axis[min_index - 1] < penalty_axis[min_index + 1]:
return (min_index, min_index - 1)
else:
return (min_index, min_index + 1)
def linear_angle_approx(first: Point2D, second: Point2D,
x: float) -> float:
""" Linear approximation from two points """
point_dx = angle_diff(first.x, second.x)
dx = angle_diff(first.x, x)
point_dy = second.y - first.y
return first.y + dx / point_dx * point_dy
i_azimut0, i_azimut1 = get_azimut_index_pair(self._azimuts, azimut)
i_elev0, i_elev1 = get_elevation_index_pair(self._elevations,
elevation)
first = Point2D(self._azimuts[i_azimut0], self._intenzities[i_azimut0,
i_elev0])
second = Point2D(self._azimuts[i_azimut1], self._intenzities[i_azimut1,
i_elev0])
intenzity_elev0 = linear_angle_approx(first, second, azimut)
if i_elev1:
first = Point2D(self._azimuts[i_azimut0],
self._intenzities[i_azimut0, i_elev1])
second = Point2D(self._azimuts[i_azimut1],
self._intenzities[i_azimut1, i_elev1])
intenzity_elev1 = linear_angle_approx(first, second, azimut)
first = Point2D(self._elevations[i_elev0], intenzity_elev0)
second = Point2D(self._elevations[i_elev1], intenzity_elev1)
return linear_angle_approx(first, second, elevation)
else:
return intenzity_elev0
def _getStraightLine(point1: Point2D, point2: Point2D) -> list:
"""
Get line from points including the points included in the line
Bresenham's line algorithm adapted from pseudocode in wikipedia:
https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
image should be grayscale
"""
# define local variables for readability
P1X = point1.x
P1Y = point1.y
P2X = point2.x
P2Y = point2.y
# difference and absolute difference between points
# used to calculate slope and relative location between points
diffX = P2X - P1X
diffXa = np.absolute(diffX, dtype="int32")
diffY = P2Y - P1Y
diffYa = np.absolute(diffY, dtype="int32")
#
steepx = 1
if P1X < P2X:
steepx = 1
else:
steepx = -1
#
if P1Y < P2Y:
steepy = 1
else:
steepy = -1
#
div_term = diffXa
#
if diffXa > diffYa:
div_term = diffXa
else:
div_term = -diffYa
#
error = div_term / 2
#
error2 = 0
#
arrival_condition = bool((P1X, P1Y) == (P2X, P2Y))
#
line_points = []
initial_p = Point2D([P1X, P1Y])
line_points.append(initial_p)
#
while arrival_condition is False:
error2 = error
if error2 > -diffXa:
error = error - diffYa
P1X = P1X + steepx
#
if error2 < diffYa:
error = error + diffXa
P1Y = P1Y + steepy
#
# Check
point = Point2D([P1X, P1Y])
line_points.append(point)
arrival_condition = bool((P1X, P1Y) == (P2X, P2Y))
#
return line_points
