def calc_block_point(self):
ball_pos = self.bb.ball_global
if self.bb.parameters.attackRight:
goal = VecPos(-550, 0)
else:
goal = VecPos(550, 0)
# goal.x *= -1
theta = degree_to_radian(degree_between(goal, ball_pos))
block_point = VecPos()
block_point.x = ball_pos.x - AVOID_DIS * 1.5 * cos(theta)
block_point.y = ball_pos.y - AVOID_DIS * 1.5 * sin(theta)
block_point.z = degrees(theta)
return block_point
def calc_final_dest(enable_kick, ball_field, ball_global, target):
theta = angle_between(target, ball_global)
closer_to_left_foot = ball_field.y > 0
# FIXME(MWX): may blur
if not enable_kick or (not cfg.left_kick and not cfg.right_kick):
kick_point = closer_to_left_foot and cfg.left_kick_point or cfg.right_kick_point
else:
kick_point = cfg.left_kick and cfg.left_kick_point or cfg.right_kick_point
res = VecPos()
res.x = ball_global.x + kick_point.x * cos(theta)
res.y = ball_global.y + kick_point.x * sin(theta)
theta2 = angle_between(target, res)
res.z = degrees(theta2 + PI)
return res
def fast(self, localDest):
sx_star, sy_star, st_star = 0, 0, 0
mirror = False
dest = localDest.copy()
if dest.y < 0:
dest.y = -dest.y
dest.z = -dest.z
mirror = True
xInput = self.vx
thetaP = dest.slope()
thetaAttack = dest.z
thetaSafe = min(thetaP, THETA_SAFE)
thetaAttackP = thetaAttack - thetaP
aDest = dest.copy()
aDest.rotate(90)
if thetaAttackP > thetaP:
thetaMax = thetaP
elif thetaAttackP > thetaSafe:
thetaMax = thetaAttackP
else:
thetaMax = thetaSafe
thetaMax = min(thetaMax, thetaSafe)
thetaMax = min(thetaMax, thetaP)
dt = 1
stepnumMin = 999999
i = 0
if abs(thetaMax) < TOP_THETA_MAX:
sx_star, sy_star, st_star = TOP_X_MAX, 0, thetaMax
else:
cross_i = VecPos()
while i < thetaMax:
a_i = aDest.copy()
a_i.rotate(i)
b_i = a_i.dot(dest)
r_i = b_i / (a_i.y - a_i.length() + epso)
sx, sy, st = self.getx_max(r_i)
st = min(thetaP, st)
c_i = i + thetaP
cross_i.x = r_i * sind(c_i)
cross_i.y = r_i * (1 - cosd(c_i))
line_i = (dest - cross_i).copy()
k1 = 15.0
k2 = 10.0
stepnum_arc = c_i / (st == 0 and epso or st)
stepnum_line = line_i.length() / TOP_X_MAX
if thetaP < AD_THETA_MAX + 20:
k2 = 25.0
stepnum_speed = abs(max(-sx + xInput, 0)) / TOP_X_MAX * k1 * thetaP * pi / 180 + \
abs(TOP_X_MAX - sx) * k2 / TOP_X_MAX
stepnum = stepnum_arc + stepnum_line + stepnum_speed
if stepnumMin > stepnum:
stepnumMin = stepnum
sx_star, sy_star, st_star = sx, sy, st
i += dt
if mirror:
st_star = -st_star
# sy_star = -sy_star
# print 'fast', sx_star, sy_star, st_star, localDest.x, localDest.y, localDest.z
return sx_star, sy_star, st_star
