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