def __init__(self, a, b):
neg_x, pos_x = min(a.x, b.x), max(a.x, b.x)
neg_y, pos_y = min(a.y, b.y), max(a.y, b.y)
self.upper_left = Position(neg_x, pos_y)
self.upper_right = Position(pos_x, pos_y)
self.lower_right = Position(pos_x, neg_y)
self.lower_left = Position(neg_x, neg_y)
def intersection_between_lines(a1: Position, a2: Position, b1: Position,
b2: Position) -> Position:
s = np.vstack([a1.array, a2.array, b1.array, b2.array])
h = np.hstack((s, np.ones((4, 1))))
l1 = np.cross(h[0], h[1]) # first line
l2 = np.cross(h[2], h[3]) # second line
x, y, z = np.cross(l1, l2) # point of intersection
if z == 0:
raise ValueError("Parallel lines")
return Position(x / z, y / z)
def intersection_line_and_circle(cp: Position, cr: float, lp1: Position,
lp2: Position) -> List[Position]:
# Based on http://mathworld.wolfram.com/Circle-LineIntersection.html
lp1 = lp1.copy() - cp
lp2 = lp2.copy() - cp
d = lp2 - lp1
det = lp1.x * lp2.y - lp2.x * lp1.y
delta = cr**2 * d.norm**2 - det**2
if delta < 0:
return [] # No intersection
x1 = (det * d.y + np.sign(d.y) * d.x * np.sqrt(delta)) / (d.norm**2)
y1 = (-det * d.x + abs(d.y) * np.sqrt(delta)) / (d.norm**2)
if delta == 0:
return [Position(x1, y1) + cp] # Tangential
x2 = (det * d.y - np.sign(d.y) * d.x * np.sqrt(delta)) / (d.norm**2)
y2 = (-det * d.x - abs(d.y) * np.sqrt(delta)) / (d.norm**2)
return [Position(x1, y1) + cp, Position(x2, y2) + cp]
def __init__(self, p_game_state):
super().__init__(p_game_state)
for role, player in self.assigned_roles.items():
self.create_node(
role,
GoToRandomPosition(self.game_state,
player,
center_of_zone=Position(-0, 0),
width_of_zone=2000,
height_of_zone=2000))
def best_position_in_region(player, A, B):
# Retourne la position (dans un rectangle aux points A et B) la mieux placée pour une passe
ncounts = 5
bottom_left = Position(min(A.x, B.x), min(A.y, B.y))
top_right = Position(max(A.x, B.x), max(A.y, B.y))
ball_position = GameState().ball_position
positions = []
for i in range(ncounts):
x_point = bottom_left.x + i * (top_right.x - bottom_left.x) / (ncounts - 1)
for j in range(ncounts):
y_point = bottom_left.y + j * (top_right.y - bottom_left.y) / (ncounts - 1)
positions += [Position(x_point, y_point).array]
positions = np.stack(positions)
# la maniere full cool de calculer la norme d'un matrice verticale de vecteur horizontaux:
dists_from_ball_raw = np.sqrt(((positions - ball_position.array) *
(positions - ball_position.array)).sum(axis=1))
positions = positions[dists_from_ball_raw > 1000, :]
dists_from_ball = dists_from_ball_raw[dists_from_ball_raw > 1000]
scores = line_of_sight_clearance_ball(player, positions, dists_from_ball)
our_side = GameState().field.our_goal_x
if abs(A.x - our_side) < abs(B.x - our_side):
x_closest_to_our_side = A.x
else:
x_closest_to_our_side = B.x
width = abs(A.x - B.x)
saturation_modifier = np.clip((positions[:, 0] - x_closest_to_our_side) / width, 0.05, 1)
scores /= saturation_modifier
try:
best_score_index = np.argmin(scores)
best_position = positions[best_score_index, :]
except IndexError:
best_position = Position()
return best_position
def _update(self, referee_info: Dict) -> None:
self.stage = Stage(referee_info["stage"])
# TODO: Currently the only optional field that we might rely on is the time left
# since most packet don't specify it we need a additional state to track it
if "stage_time_left" in referee_info:
self.stage_time_left = referee_info["stage_time_left"]
if "blueTeamOnPositiveHalf" in referee_info:
self.blue_team_is_positive = referee_info["blueTeamOnPositiveHalf"]
if "designated_position" in referee_info:
self.ball_placement_position = self._convert_vision_position_to_ai_position(
Position(referee_info["designated_position"]["x"],
referee_info["designated_position"]["y"]))
raw_command = RawRefereeCommand(referee_info["command"])
if "blueTeamOnPositiveHalf" in referee_info and Config(
)['GAME']['competition_mode']:
self._validate_field_side(referee_info["blueTeamOnPositiveHalf"])
self.command = self._parse_command(raw_command)
self._parse_team_info(referee_info)
def center(self):
return self.lower_left + Position(self.width / 2, self.height / 2)
def perpendicular(vec: Position) -> Position:
return normalize(Position(-vec.y, vec.x))
def from_limits(cls, top, bottom, right, left):
return cls(Position(left, top), Position(right, bottom))
def GoBehind(player: Player, position1: Position, position2: Optional[Position]=None,
distance_behind: float=250,
orientation: str= 'front'):
"""
Action GoBehind: Déplace le robot au point le plus proche sur la droite, derrière un objet dont la position
est passée en paramètre, de sorte que cet objet se retrouve entre le robot et la seconde position passée
en paramètre
Méthodes :
exec(self): Retourne la pose où se rendre
Attributs (en plus de ceux de Action):
player_id : L'identifiant du joueur
position1 : La position de l'objet derrière lequel le robot doit se placer (exemple: le ballon)
position2 : La position par rapport à laquelle le robot doit être "derrière" l'objet de la position 1
(exemple: le but)
"""
delta_x = position2.x - position1.x
delta_y = position2.y - position1.y
theta = math.atan2(delta_y, delta_x)
x = position1.x - distance_behind * math.cos(theta)
y = position1.y - distance_behind * math.sin(theta)
player_x = player.pose.position.x
player_y = player.pose.position.y
norm_player_2_position2 = math.sqrt((player_x - position2.x) ** 2+(player_y - position2.y) ** 2)
norm_position1_2_position2 = math.sqrt((position1.x - position2.x) ** 2 +
(position1.y - position2.y) ** 2)
if norm_player_2_position2 < norm_position1_2_position2:
# on doit contourner l'objectif
vecteur_position1_2_position2 = np.array([position1.x - position2.x,
position1.y - position2.y, 0])
vecteur_vertical = np.array([0, 0, 1])
vecteur_player_2_position1 = np.array([position1.x - player_x,
position1.y - player_y, 0])
vecteur_perp = np.cross(vecteur_position1_2_position2, vecteur_vertical)
vecteur_perp /= np.linalg.norm(vecteur_perp)
if np.dot(vecteur_perp, vecteur_player_2_position1) > 0:
vecteur_perp = -vecteur_perp
position_intermediaire_x = x + vecteur_perp[0] * RAYON_AVOID
position_intermediaire_y = y + vecteur_perp[1] * RAYON_AVOID
if math.sqrt((player_x-position_intermediaire_x)**2+(player_y-position_intermediaire_y)**2) < 50:
position_intermediaire_x += vecteur_perp[0] * RAYON_AVOID * 2
position_intermediaire_y += vecteur_perp[1] * RAYON_AVOID * 2
destination_position = Position(position_intermediaire_x, position_intermediaire_y)
else:
if math.sqrt((player_x-x)**2+(player_y-y)**2) < 50:
x -= math.cos(theta) * 2
y -= math.sin(theta) * 2
destination_position = Position(x, y)
# Calcul de l'orientation de la pose de destination
destination_orientation = 0
if orientation == 'front':
destination_orientation = wrap_to_pi((position1 - destination_position).angle)
elif orientation == 'back':
destination_orientation = wrap_to_pi((position1 - destination_position).angle + np.pi)
destination_pose = Pose(destination_position, destination_orientation)
return MoveTo(destination_pose)