def predict_puck_movement(self, bag): #ausgefuehrt
"""
Predicts the pucks movement.
:param bag: The parameter bag.
:return:
"""
# need at least 2 points
if len(self.buffer) < 2:
return
# loop over points
direction_sum = (0, 0)
for i in xrange(0, len(self.buffer) - 1):
# get tuples and time diff
current_tuple = self.buffer[i]
next_tuple = self.buffer[i+1]
time_diff = next_tuple[2] - current_tuple[2]
# get direction (length is velocity) and divide by time_diff
direction = vector.mul_by(vector.from_to(current_tuple, next_tuple), 1.0 / time_diff)
# sum up
direction_sum = vector.add(direction_sum, direction)
# averaging
direction = vector.mul_by(direction_sum, 1.0 / self.buffer_size)
# add puck direction (normalized) and velocity
bag.puck.velocity = vector.length(direction)
bag.puck.direction = vector.normalize(direction)
bag.next.puck.olddirection = bag.puck.direction
def draw_puck(self, bag):
"""
Draws the puck into the image.
:param bag:
:return:
"""
if 'position' in bag.puck:
# draw puck itself
color = ((255, 0, 0) if bag.puck.is_predicted_position else (0, 255, 0))
cv2.circle(bag.image, bag.puck.position_pixel, bag.puck.radius_unscaled, color, 2)
cv2.circle(bag.image, bag.puck.position_pixel, 2, (0, 0, 255), 3)
if 'path' in bag.puck:
# draw puck movement
#direction_velocity = vector.mul_by(bag.puck.direction, bag.puck.velocity)
#destination = vector.add(bag.puck.position, direction_velocity)
#destination_pixel = vector.coord_to_image_space(destination, bag.table_boundaries)
#cv2.line(bag.image, bag.puck.position_pixel, destination_pixel, (255, 0, 0))
# draw path
for i in xrange(0, len(bag.puck.path) - 1):
position = vector.coord_to_image_space(bag.puck.path[i][0], bag.table_boundaries)
destination = vector.coord_to_image_space(bag.puck.path[i + 1][0], bag.table_boundaries)
cv2.line(bag.image, position, destination, (255, 0, 0))
cv2.circle(bag.image, position, 1, (0, 0, 255), 2)
position_pixel = vector.coord_to_image_space(bag.puck.path[-1][0], bag.table_boundaries)
direction_velocity = vector.mul_by(bag.puck.path[-1][1], bag.puck.path[-1][2])
destination = vector.add(bag.puck.path[-1][0], direction_velocity)
destination_pixel = vector.coord_to_image_space(destination, bag.table_boundaries)
cv2.line(bag.image, position_pixel, destination_pixel, (255, 0, 0))
def move_stick(self, bag):
"""
Moves the stick.
:param bag: The parameter bag.
:return:
"""
if 'position' not in bag.stick:
bag.next.stick.position = strategy.SimpleStrategy.HOME_POSITION
return
if 'dest_position' not in bag.stick:
position = bag.stick.position
else:
# move stick towards destination
# ignore dest_direction and dest_velocity
dist_moved = strategy.SimpleStrategy.STICK_MAX_SPEED * bag.time_diff
direction = vector.from_to(bag.stick.position, bag.stick.dest_position)
if dist_moved > vector.length(direction):
position = bag.stick.dest_position
else:
direction = vector.mul_by(vector.normalize(direction), dist_moved * strategy.SimpleStrategy.STICK_MAX_SPEED)
position = vector.add(bag.stick.position, direction)
# set new position
bag.next.stick.position = position
def _predict_position(self, bag, current_time):
"""
Guesses the puck's position.
:param bag: The parameter bag
:param current_time: The current time.
:return:
"""
# only possible if we have a path and a velocity
if len(self.last_path) > 0:
# find path part in which the the puck should be by now
path_part_index = 0
while path_part_index < len(self.last_path):
path_part = self.last_path[path_part_index]
if path_part[3] > current_time:
# collision time is in future -> take last part
path_part_index -= 1
break
path_part_index += 1
path_part = self.last_path[path_part_index - 1]
# predict position
remaining_time = current_time - path_part[3]
direction = vector.mul_by(path_part[1], path_part[2] * remaining_time)
bag.puck.predicted_position = vector.add(path_part[0], direction)
elif 'position' in bag.puck:
# use actual position if found
bag.puck.predicted_position = bag.puck.position
def build_puck_path(self, bag, remaining_forecast_time=None): #ausgefuehrt
"""
Checks for a collision with the table boundaries.
:param bag: The parameter bag.
:param remaining_forecast_time:
:return:
"""
if 'direction' not in bag.puck:
return
if remaining_forecast_time is None:
remaining_forecast_time = const.CONST.TABLE_BOUNDARIES_COLLISION_FORECAST_TIME
if 'path' not in bag.puck:
bag.puck.path = []
if len(bag.puck.path) == 0:
bag.puck.path.append((bag.puck.position, bag.puck.direction, bag.puck.velocity, bag.puck.detection_time))
# get position and direction for the current path part
position = bag.puck.path[-1][0]
direction = bag.puck.path[-1][1]
velocity = bag.puck.path[-1][2]
# follow puck movement for remaining time
movement = vector.mul_by(direction, velocity)
predicted_position = vector.add(position, vector.mul_by(movement, remaining_forecast_time))
# check if position is out of bounds
collision = self._check_collision(position, predicted_position, movement)
if collision is not None:
# add to path
bag.puck.path.append((collision[0], vector.normalize(collision[1]), velocity, bag.puck.path[-1][3] + collision[2]))
# continue building path
remaining_forecast_time -= collision[2]
#print("Velocity: " + str(velocity))
#print(remaining_forecast_time)
#if len(bag.puck.path) > 20:
# exit()
if remaining_forecast_time > 0:
self.build_puck_path(bag, remaining_forecast_time)
# save path
#print("done, path: " + str(bag.puck.path))
self.last_path = bag.puck.path
def _calculate_collision_point(self, bag):
"""
Calculates the collision point between the puck and the stick.
:param bag: The parameter bag.
:return:
"""
# NOTE: First attempt, ignoring stick and puck acceleration, puck radius and stick radius here
# OK, math first:
#
# S = stick position
# C = point of collision
# v = max speed of the stick
# P = puck position
# p = puck direction * velocity
#
# Points which the stick can reach in time t build a circle around him:
# 1. (Cx - Sx)^2 + (Cy - Sy)^2 = r = v * t
#
# Points which the puck reaches in time t are on the line:
# 2. Px + px * t = Cx
# 3. Py + py * t = Cy
#
# Approach:
# - replace Cx and Cy in 1. with 2. and 3.
# - bracket the square terms (Px + px * t - Sx)^2 and (Py + py * t - Sy)^2
# - outline t^2 and t to get a formula like t^2*(...) + t*(...) + (...)
# - name (...) a, b and c
# - use the quadratic formula to get t
# - use puck movement and t to get collision point
# - use stick position and collision point
# - gg
Sx = bag.stick.position[0]
Sy = bag.stick.position[1]
Px = bag.puck.position[0]
Py = bag.puck.position[1]
px = bag.puck.direction[0] * bag.puck.velocity
py = bag.puck.direction[1] * bag.puck.velocity
v = self.STICK_MAX_SPEED
# calculate a, b and c
a = px*px + py*py - v*v
b = 2*Px*px - 2*Sx*px + 2*Py*py - 2*Sy*py
c = Px*Px - 2*Px*Sx + Sx*Sx + Py*Py - 2*Py*Sy + Sy*Sy
# calculate t
inner_sqrt = b*b - 4*a*c
if inner_sqrt < 0:
print("Going home, inner sqrt: " + str(inner_sqrt))
# no chance to get that thing, go home
self.go_home(bag)
return
# use + first, since a is negative (Vpuck - Vstick)
# (... / 2 * a) -> shortest time needed
sqrt_b2_4ac = math.sqrt(inner_sqrt)
t = (-b + sqrt_b2_4ac) / 2*a
print("Vp: "+str(bag.puck.velocity))
print("t: "+str(t))
if t < 0:
# too late for that chance, use other result
t = (-b - sqrt_b2_4ac) / 2*a
print("t2: "+str(t))
# get collision point
C = vector.add((Px, Py), vector.mul_by((px, py), t))
s = vector.from_to((Sx, Sy), C)
# save
bag.stick.dest_position = C
bag.stick.dest_direction = vector.normalize(s)
bag.stick.dest_velocity = self.STICK_MAX_SPEED
bag.stick.dest_time = time.time() + t
