def block_line(self, value):
self._block_line = value
# we move somewhere along this arc to mark our 'block_line'
arc_left = robocup.Arc(
robocup.Point(-constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2,
math.pi / 2, math.pi)
arc_right = robocup.Arc(
robocup.Point(constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2, 0,
math.pi / 2)
seg = robocup.Segment(
robocup.Point(
-constants.Field.GoalFlat / 2,
constants.Field.ArcRadius + constants.Robot.Radius * 2),
robocup.Point(
constants.Field.GoalFlat / 2,
constants.Field.ArcRadius + constants.Robot.Radius * 2))
default_pt = seg.center()
if self._block_line != None:
# main.system_state().draw_line(self._block_line, constants.Colors.White, "SubmissiveDefender")
main.system_state().draw_circle(self._block_line.get_pt(0), 0.1,
constants.Colors.White,
"SubmissiveDefender")
threat_point = self._block_line.get_pt(0)
intersection_center = seg.line_intersection(self._block_line)
if threat_point.x < 0:
intersections_left = arc_left.intersects_line(self._block_line)
if len(intersections_left) > 0:
self._move_target = max(intersections_left,
key=lambda p: p.y)
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
elif threat_point.x >= 0:
intersections_right = arc_right.intersects_line(
self._block_line)
if len(intersections_right) > 0:
self._move_target = max(intersections_right,
key=lambda p: p.y)
elif intersection_center is not None:
self._move_target = intersection_center
else:
self._move_target = default_pt
else:
self._move_target = default_pt
def execute_marking(self):
move = self.subbehavior_with_name('move')
move.pos = self.move_target
arc_left = robocup.Arc(
robocup.Point(-constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2,
math.pi / 2, math.pi)
arc_right = robocup.Arc(
robocup.Point(constants.Field.GoalFlat / 2, 0),
constants.Field.ArcRadius + constants.Robot.Radius * 2, 0,
math.pi / 2)
seg = robocup.Segment(
robocup.Point(
-constants.Field.GoalFlat / 2,
constants.Field.ArcRadius + constants.Robot.Radius * 2),
robocup.Point(
constants.Field.GoalFlat / 2,
constants.Field.ArcRadius + constants.Robot.Radius * 2))
if move.pos != None:
main.system_state().draw_circle(move.pos, 0.02,
constants.Colors.Green, "Mark")
main.system_state().draw_segment(seg, constants.Colors.Green,
"Mark")
main.system_state().draw_arc(arc_left, constants.Colors.Green,
"Mark")
main.system_state().draw_arc(arc_right, constants.Colors.Green,
"Mark")
# make the defender face the threat it's defending against
if self.robot != None and self.block_line != None:
self.robot.face(self.block_line.get_pt(0))
if self.robot.has_ball():
self.robot.kick(0.75)
def get_circle_points(self, num_of_points):
radius = constants.Field.CenterRadius + constants.Robot.Radius + 0.01
ball_pos = main.ball().pos if main.ball() != None else robocup.Point(
constants.Field.Width / 2, constants.Field.Length / 2)
circle_ball = robocup.Circle(ball_pos, radius)
intersection_points = []
for i in constants.Field.FieldBorders:
for j in circle_ball.intersects_line(i):
# Using near_point because of rounding errors
if constants.Field.FieldRect.near_point(j, 0.001):
intersection_points.append(j)
angles = []
candidate_arcs = []
if len(intersection_points) > 1:
for i in intersection_points:
new_angle = (i - circle_ball.center).angle()
new_angle = self.normalize_angle(new_angle)
angles.append(new_angle)
# Get angles going sequentially
angles.sort()
counter = 1
while counter < len(angles):
candidate_arcs.append(robocup.Arc(circle_ball.center, radius,
angles[counter - 1], angles[
counter]))
counter = counter + 1
candidate_arcs.append(robocup.Arc(circle_ball.center, radius,
angles[len(angles) - 1], angles[
0]))
i = 0
while i < len(candidate_arcs):
angle_between = candidate_arcs[i].end() - candidate_arcs[
i].start()
angle_between = self.normalize_angle(angle_between)
angle_diff = candidate_arcs[i].start() + (angle_between) / 2.0
angle_diff = self.normalize_angle(angle_diff)
midpoint = (
candidate_arcs[i].center() + robocup.Point(radius, 0))
midpoint.rotate(candidate_arcs[i].center(), angle_diff)
if not constants.Field.FieldRect.contains_point(midpoint):
candidate_arcs.pop(i)
else:
i = i + 1
candidate_arcs.sort(
key=lambda arc: self.normalize_angle(arc.end() - arc.start()),
reverse=True)
if len(candidate_arcs) <= 0:
final_arc = robocup.Arc(CircleNearBall.BackupBallLocation,
radius, math.pi / 2, 5 * math.pi / 2)
else:
final_arc = candidate_arcs[0]
else:
midpoint = (circle_ball.center + robocup.Point(radius, 0))
if not constants.Field.FieldRect.contains_point(midpoint):
final_arc = robocup.Arc(CircleNearBall.BackupBallLocation,
radius, math.pi / 2, 5 * math.pi / 2)
else:
final_arc = robocup.Arc(circle_ball.center, radius,
math.pi / 2, 5 * math.pi / 2)
arc_angle = final_arc.end() - final_arc.start()
arc_angle = self.normalize_angle(arc_angle)
perRobot = arc_angle / (num_of_points + 1)
dirvec = robocup.Point(radius, 0)
dirvec.rotate(robocup.Point(0, 0), final_arc.start())
dirvec.rotate(robocup.Point(0, 0), perRobot)
final_points = []
for i in range(num_of_points):
pt = final_arc.center() + dirvec
final_points.append(pt)
dirvec.rotate(robocup.Point(0, 0), perRobot)
return final_points