def run(self, drone: CarObject, agent: MyHivemind):
# An example of pushing routines to the stack:
agent.line(Vector3(0, 0, 50), 2000 * self.vector.flatten(), color=[255, 0, 0])
agent.line(Vector3(0, 0, 50), 2000 * drone.forward.flatten(), color=[0, 255, 0])
robbies_constant = (self.vector * 1.5 * 2200 - drone.velocity * 1.5) * 2 * 1.5 ** -2
robbies_boost_constant = drone.forward.flatten().normalize().dot(robbies_constant.flatten().normalize()) > (
0.3 if not drone.airborne else 0.1)
drone.controller.boost = robbies_boost_constant and self.boost and not drone.supersonic
if self.time == -1:
elapsed = 0
self.time = agent.time
else:
elapsed = agent.time - self.time
if elapsed < self.delay:
if elapsed < self.duration:
drone.controller.jump = True
else:
drone.controller.jump = False
self.counter += 1
defaultPD(drone, self.preorientation)
elif elapsed >= self.delay and self.counter < 3:
drone.controller.jump = False
defaultPD(drone, self.preorientation)
self.counter += 1
elif elapsed < self.delay + 0.05:
drone.controller.jump = True
defaultPD(drone, self.vector)
else:
drone.pop()
drone.push(Recovery(boost=self.boost, time=agent.time))
def defaultPD(drone: CarObject,
local_target: Vector3,
upside_down=False,
up=None) -> []:
# points the car towards a given local target.
# Direction can be changed to allow the car to steer towards a target while driving backwards
if up is None:
up = drone.local(
Vector3(0, 0, -1) if upside_down else Vector3(
0, 0, 1)) # where "up" is in local coordinates
target_angles = (
math.atan2(local_target[2],
local_target[0]), # angle required to pitch towards target
math.atan2(local_target[1],
local_target[0]), # angle required to yaw towards target
math.atan2(up[1], up[2]) # angle required to roll upright
)
# Once we have the angles we need to rotate, we feed them into PD loops to determining the controller inputs
drone.controller.steer = steerPD(target_angles[1], 0)
drone.controller.pitch = steerPD(target_angles[0],
drone.angular_velocity[1] / 4)
drone.controller.yaw = steerPD(target_angles[1],
-drone.angular_velocity[2] / 4)
drone.controller.roll = steerPD(target_angles[2],
drone.angular_velocity[0] / 4)
# Returns the angles, which can be useful for other purposes
return target_angles
def run(self, drone: CarObject, agent: MyHivemind):
car_to_ball, distance = (agent.ball.location - drone.location).normalize(True)
ball_to_target = (self.target - agent.ball.location).normalize()
relative_velocity = car_to_ball.dot(drone.velocity - agent.ball.velocity)
if relative_velocity != 0.0:
eta = cap(distance / cap(relative_velocity, 400, 2300), 0.0, 1.5)
else:
eta = 1.5
# If we are approaching the ball from the wrong side the car will try to only hit the very edge of the ball
left_vector = car_to_ball.cross((0, 0, 1))
right_vector = car_to_ball.cross((0, 0, -1))
target_vector = -ball_to_target.clamp(left_vector, right_vector)
final_target = agent.ball.location + (target_vector * (distance / 2))
# Some adjustment to the final target to ensure we don't try to drive through any goalposts to reach it
if abs(drone.location[1]) > 5150:
final_target[0] = cap(final_target[0], -750, 750)
agent.line(final_target - Vector3(0, 0, 100), final_target + Vector3(0, 0, 100), [255, 255, 255])
angles = defaultPD(drone, drone.local(final_target - drone.location))
defaultThrottle(drone, 2300 if distance > 1600 else 2300 - cap(1600 * abs(angles[1]), 0, 2050))
drone.controller.boost = False if drone.airborne or abs(angles[1]) > 0.3 else drone.controller.boost
drone.controller.handbrake = True if abs(angles[1]) > 2.3 else drone.controller.handbrake
if abs(angles[1]) < 0.05 and (eta < 0.45 or distance < 150):
drone.pop()
drone.push(Flip(drone.local(car_to_ball)))
def shot_valid(agent: MyHivemind,
shot: Union[AerialShot, JumpShot, Aerial],
threshold: float = 45) -> bool:
# Returns True if the ball is still where the shot anticipates it to be
# First finds the two closest slices in the ball prediction to shot's intercept_time
# threshold controls the tolerance we allow the ball to be off by
slices = agent.get_ball_prediction_struct().slices
soonest = 0
latest = len(slices) - 1
while len(slices[soonest:latest + 1]) > 2:
midpoint = (soonest + latest) // 2
if slices[midpoint].game_seconds > shot.intercept_time:
latest = midpoint
else:
soonest = midpoint
# preparing to interpolate between the selected slices
dt = slices[latest].game_seconds - slices[soonest].game_seconds
time_from_soonest = shot.intercept_time - slices[soonest].game_seconds
slopes = (Vector3(slices[latest].physics.location) -
Vector3(slices[soonest].physics.location)) * (1 / dt)
# Determining exactly where the ball will be at the given shot's intercept_time
predicted_ball_location = Vector3(
slices[soonest].physics.location) + (slopes * time_from_soonest)
# Comparing predicted location with where the shot expects the ball to be
return (shot.ball_location -
predicted_ball_location).magnitude() < threshold
def bestShotVector(car, ball_location):
relative = (ball_location - car.location)
left_post_vector = Vector3(750 * -side(car.team), 5150 * -side(car.team),
100) - ball_location
right_post_vector = Vector3(750 * side(car.team), 5150 * -side(car.team),
100) - ball_location
return (relative).clamp(left_post_vector, right_post_vector).normalize()
def find_slope(shot_vector: Vector3, car_to_target: Vector3) -> float:
# Finds the slope of your car's position relative to the shot vector (shot vector is y axis)
# 10 = you are on the axis and the ball is between you and the direction to shoot in
# -10 = you are on the wrong side
# 1.0 = you're about 45 degrees offcenter
d = shot_vector.dot(car_to_target)
e = abs(shot_vector.cross((0, 0, 1)).dot(car_to_target))
return cap(d / e if e != 0 else 10 * sign(d), -3.0, 3.0)
def run(self, drone: CarObject, agent: MyHivemind):
target = Vector3(0, 3800 * agent.side(), 0)
local_target = drone.local(target - drone.location)
defaultPD(drone, local_target)
defaultThrottle(drone, 2300)
if local_target.magnitude() < 100:
drone.pop()
drone.push(DiagonalKickoff())
drone.push(Flip(Vector3(1, 0, 0)))
def align(point_location, ball_location, goal_location):
position_to_ball = (ball_location - point_location).normalize().flatten()
ball_to_goal_center = (goal_location - ball_location).normalize().flatten()
ball_to_goal_left_post = (goal_location + Vector3(800, 0, 0) -
ball_location).normalize().flatten()
ball_to_goal_right_post = (goal_location + Vector3(-800, 0, 0) -
ball_location).normalize().flatten()
best_case = min(position_to_ball.dot(ball_to_goal_center),
position_to_ball.dot(ball_to_goal_right_post),
position_to_ball.dot(ball_to_goal_left_post))
return best_case
def find_slope(shot_vector: Vector3, car_to_target: Vector3) -> float:
# Finds the slope of your car's position relative to the shot vector (shot vector is y axis)
# 10 = you are on the axis and the ball is between you and the direction to shoot in
# -10 = you are on the wrong side
# 1 = you're about 45 degrees offcenter
d = shot_vector.dot(car_to_target)
e = abs(shot_vector.cross(Vector3(0, 0, 1)).dot(car_to_target))
try:
f = d / e
except ZeroDivisionError:
return 10 * sign(d)
return cap(f, -3, 3)
def distance_to_wall(point):
#determines how close the car is to the wall
point = Vector3(abs(point[0]), abs(point[1]), abs(point[2]))
if 4096 - point[0] < 5120 - point[1]:
return 4096 - point[0]
else:
return 5120 - point[1]
def backsolve(target, car, time, gravity=650):
#Finds the acceleration required for a car to reach a target in a specific amount of time
d = target - car.location
dvx = ((d[0] / time) - car.velocity[0]) / time
dvy = ((d[1] / time) - car.velocity[1]) / time
dvz = (((d[2] / time) - car.velocity[2]) / time) + (gravity * time)
return Vector3(dvx, dvy, dvz)
def shotValid(slices, shot):
mi = 0
ma = len(slices) - 1
while len(slices[mi:ma + 1]) > 2:
if slices[(ma + mi) // 2].game_seconds > shot.intercept_time:
ma = (ma + mi) // 2
else:
mi = (ma + mi) // 2
dt = slices[ma].game_seconds - slices[mi].game_seconds
time_from_mi = shot.intercept_time - slices[mi].game_seconds
mi = slices[mi].physics.location
ma = slices[ma].physics.location
slopes = Vector3(ma.x - mi.x, ma.y - mi.y, ma.z - mi.z) * (1 / dt)
slice_intercept = Vector3(mi.x, mi.y, mi.z) + (slopes * time_from_mi)
if (shot.ball - slice_intercept).magnitude() > 30:
return False
return True
def get_closest_post_vector(agent, future_ball_location=None):
y = 4400 * utils.side(agent.team)
z = 0
closest_post = closest_point(
agent.me.location,
[agent.friend_goal.left_post, agent.friend_goal.right_post])
return_value = Vector3(closest_post.x, y, z)
return return_value
def run(self, drone: CarObject, agent: MyHivemind):
target = agent.ball.location + Vector3(0, 200 * agent.side(), 0)
local_target = drone.local(target - drone.location)
defaultPD(drone, local_target)
defaultThrottle(drone, 2300)
if local_target.magnitude() < 650:
drone.pop()
drone.push(Flip(drone.local(agent.foe_goal.location - drone.location)))
def __init__(self, vector: Vector3, duration: float = 0.1, delay: float = 0.1, angle: float = 0,
boost: bool = False):
super().__init__()
self.vector = vector.normalize()
self.pitch = abs(self.vector[0]) * -sign(self.vector[0])
self.yaw = abs(self.vector[1]) * sign(self.vector[1])
self.delay = delay if delay >= duration else duration
self.duration = duration
self.boost = boost
self.angle = math.radians(angle) if boost else 0
x = math.cos(self.angle) * self.vector.x - math.sin(self.angle) * self.vector.y
y = math.sin(self.angle) * self.vector.x + math.cos(self.angle) * self.vector.y
self.preorientation = Vector3(x, y, 0)
# the time the jump began
self.time = -1
# keeps track of the frames the jump button has been released
self.counter = 0
def push_shot(drone: CarObject, agent: MyHivemind):
left = Vector3(4200 * -agent.side(),
agent.ball.location.y + (1000 * -agent.side()), 0)
right = Vector3(4200 * agent.side(),
agent.ball.location.y + (1000 * -agent.side()), 0)
targets = {
"goal": (agent.foe_goal.left_post, agent.foe_goal.right_post),
"upfield": (left, right)
}
shots = find_hits(drone, agent, targets)
if len(shots["goal"]) > 0:
drone.clear()
drone.push(shots["goal"][0])
drone.action = Action.Going
elif len(shots["upfield"]) > 0:
drone.clear()
drone.push(shots["upfield"][0])
drone.action = Action.Going
def backsolve(target: Vector3,
car: CarObject,
time: float,
gravity: int = 650) -> Vector3:
# Finds the acceleration required for a car to reach a target in a specific amount of time
d = target - car.location
dvx = ((d[0] / time) - car.velocity[0]) / time
dvy = ((d[1] / time) - car.velocity[1]) / time
dvz = (((d[2] / time) - car.velocity[2]) / time) + (gravity * time)
return Vector3(dvx, dvy, dvz)
def defaultPD(agent, local, direction=0):
yaw = math.atan2(local[1], local[0])
turn = (math.pi * direction) + yaw if direction != 0 else yaw
up = agent.me.matrix.dot(Vector3(0, 0, 1))
target = [math.atan2(up[1], up[2]), math.atan2(local[2], local[0]), turn]
agent.c.steer = steerPD(turn, 0)
agent.c.yaw = steerPD(target[2], -agent.me.rvel[2] / 4)
agent.c.pitch = steerPD(target[1], agent.me.rvel[1] / 4)
agent.c.roll = steerPD(target[0], agent.me.rvel[0] / 2.5)
return target
def run(self, drone: CarObject, agent: MyHivemind):
if self.boost is None:
drone.pop()
return
car_to_boost = self.boost.location - drone.location
distance_remaining = car_to_boost.flatten().magnitude()
agent.line(self.boost.location - Vector3(0, 0, 500), self.boost.location + Vector3(0, 0, 500), [0, 255, 0])
if self.target is not None:
vector = (self.target - self.boost.location).normalize()
side_of_vector = sign(vector.cross((0, 0, 1)).dot(car_to_boost))
car_to_boost_perp = car_to_boost.cross((0, 0, side_of_vector)).normalize()
adjustment = car_to_boost.angle2D(vector) * distance_remaining / 3.14
final_target = self.boost.location + (car_to_boost_perp * adjustment)
car_to_target = (self.target - drone.location).magnitude()
else:
adjustment = 9999
car_to_target = 0
final_target = self.boost.location
# Some adjustment to the final target to ensure it's inside the field and
# we don't try to dirve through any goalposts to reach it
if abs(drone.location[1]) > 5150:
final_target[0] = cap(final_target[0], -750, 750)
local_target = drone.local(final_target - drone.location)
angles = defaultPD(drone, local_target)
defaultThrottle(drone, 2300)
drone.controller.boost = self.boost.large if abs(angles[1]) < 0.3 else False
drone.controller.handbrake = True if abs(angles[1]) > 2.3 else drone.controller.handbrake
velocity = 1 + drone.velocity.magnitude()
if not self.boost.active or drone.boost >= 99.0 or distance_remaining < 350:
drone.pop()
elif drone.airborne:
drone.push(Recovery(self.target))
elif abs(angles[1]) < 0.05 and 600 < velocity < 2150 and (
distance_remaining / velocity > 2.0 or (adjustment < 90 and car_to_target / velocity > 2.0)):
drone.push(Flip(local_target))
def get_back_post_vector(agent, future_ball_location=None):
#returns the location of our goal's back post.
y = 4400 * utils.side(agent.team)
z = 0
ball_location = future_ball_location if future_ball_location is not None else agent.ball.location
if ball_location[0] > 0:
x = -850
else:
x = +850
back_post = Vector3(x, y, z)
return back_post
def __init__(self, vector: Vector3, duration: float = 0.1, delay: float = 0.1):
super().__init__()
self.vector = vector.normalize()
self.pitch = abs(self.vector[0]) * -sign(self.vector[0])
self.yaw = abs(self.vector[1]) * sign(self.vector[1])
self.delay = delay if delay >= duration else duration
self.duration = duration
# the time the jump began
self.time = -1
# keeps track of the frames the jump button has been released
self.counter = 0
def field(point, radius):
point = Vector3(abs(point[0]), abs(point[1]), abs(point[2]))
if point[0] > 3860 - radius:
return False
elif point[1] > 5800 - radius:
return False
elif point[0] > 820 - radius and point[1] > 4950 - radius:
return False
elif point[0] > 2800 - radius and point[1] > -point[0] + 7750 - radius:
return False
return True
def test_setGet(self):
vec = Vector3(0, 0, 0)
vec.x = 1
self.assertEqual(vec.x, 1)
self.assertEqual(vec[0], 1)
vec.y = 1
self.assertEqual(vec.y, 1)
self.assertEqual(vec[1], 1)
vec.z = 1
self.assertEqual(vec.z, 1)
self.assertEqual(vec[2], 1)
def ball_going_into_their_danger_zone(agent):
slices = get_slices(agent, 6)
if slices is not None:
for slice in slices:
ball_location = Vector3(slice.physics.location)
if side(
agent.team
) * ball_location.y < -3600 and -1600 < ball_location.x < 1600:
return True
return False
def in_field(point: Vector3, radius: float) -> bool:
# determines if a point is inside the standard soccer field
point = Vector3(abs(point[0]), abs(point[1]), abs(point[2]))
if point[0] > 4080 - radius:
return False
elif point[1] > 5900 - radius:
return False
elif point[0] > 880 - radius and point[1] > 5105 - radius:
return False
elif point[0] > 2650 and point[1] > -point[0] + 8025 - radius:
return False
return True
def shotFinder(agent):
shots = []
struct = agent.get_ball_prediction_struct()
for i in range(18, struct.num_slices, 18):
intercept_time = struct.slices[i].game_seconds
time_remaining = intercept_time - agent.time
temp = struct.slices[i].physics.location
ball = Vector3(temp.x, temp.y, temp.z)
if (ball - agent.me.location).magnitude() / time_remaining < 2250:
ratio = shotConeRatio(agent, agent.me, ball, True)
if ratio < -0.1:
upfield_vector = Vector3(0, 1.0 * -side(agent.team), 0)
intercept = ball - (93 * upfield_vector)
shots.append(
shotObject(intercept, upfield_vector, intercept_time,
True))
if ratio < -0.5:
shot_vector = bestShotVector(agent.me, ball)
intercept = ball - (93 * shot_vector)
shots.append(
shotObject(intercept, shot_vector, intercept_time, False))
return shots
def run(self, drone: CarObject, agent: MyHivemind):
target = agent.friend_goal.location + (agent.ball.location - agent.friend_goal.location) / 2
car_to_target = target - drone.location
distance_remaining = car_to_target.flatten().magnitude()
agent.line(target - Vector3(0, 0, 500), target + Vector3(0, 0, 500), [255, 0, 255])
if self.vector is not None:
# See commends for adjustment in jump_shot or aerial for explanation
side_of_vector = sign(self.vector.cross((0, 0, 1)).dot(car_to_target))
car_to_target_perp = car_to_target.cross((0, 0, side_of_vector)).normalize()
adjustment = car_to_target.angle(self.vector) * distance_remaining / 3.14
final_target = target + (car_to_target_perp * adjustment)
else:
final_target = target
# Some adjustment to the final target to ensure it's inside the field and
# we don't try to drive through any goalposts to reach it
if abs(drone.location[1]) > 5150:
final_target[0] = cap(final_target[0], -750, 750)
local_target = drone.local(final_target - drone.location)
angles = defaultPD(drone, local_target, self.direction)
defaultThrottle(drone, 2300, self.direction)
drone.controller.boost = False
drone.controller.handbrake = True if abs(angles[1]) > 2.3 else drone.controller.handbrake
velocity = 1 + drone.velocity.magnitude()
if distance_remaining < 350:
drone.pop()
elif abs(angles[1]) < 0.05 and 600 < velocity < 2150 and distance_remaining / velocity > 2.0:
drone.push(Flip(local_target))
# TODO Halfflip
# elif abs(angles[1]) > 2.8 and velocity < 200:
# agent.push(flip(local_target, True))
elif drone.airborne:
drone.push(Recovery(target))
def find_intercept_time_with_detour(car,
agent,
ball_prediction_slices=None,
return_intercept_location_too=False,
time_limit: float = None,
time_to_subtract=0.0,
ball_height_max=9999):
#find the earliest time that a car can intercept the ball. Returns None otherwise.
earliest_intercept_time = None
earliest_intercept_location = None
ball_prediction_struct = ball_prediction_slices if ball_prediction_slices is not None else get_slices(
agent, 6)
if ball_prediction_struct is not None:
for slice in ball_prediction_slices:
prediction_slice = slice
intercept_time = prediction_slice.game_seconds
time_remaining = intercept_time - agent.time - time_to_subtract
if time_limit is not None:
if intercept_time - agent.time > time_limit:
break
if time_remaining > 0:
current_prediction_slice_ball_location = Vector3(
prediction_slice.physics.location.x,
prediction_slice.physics.location.y,
prediction_slice.physics.location.z)
reposition_target = find_reposition_target(
agent, current_prediction_slice_ball_location)
time_to_reach_intercept = time_to_reach_multiple_locations(
car, [
reposition_target,
current_prediction_slice_ball_location
])
if time_to_reach_intercept < time_remaining:
earliest_intercept_time = intercept_time
earliest_intercept_location = current_prediction_slice_ball_location
#print("Time to run non np code: " + str(toc - tic))
if return_intercept_location_too:
return earliest_intercept_time, earliest_intercept_location
else:
return earliest_intercept_time
#print("Time to run non np code: " + str(toc - tic))
if return_intercept_location_too:
return None, None
else:
return None
def defaultPD(agent, local, direction=1.0):
#to reverse, multiply local by -1.0 and agent.c.steer by -1.0
local *= direction
up = agent.me.matrix.dot(Vector3(0, 0, 1))
target = [
math.atan2(up[1], up[2]),
math.atan2(local[2], local[0]),
math.atan2(local[1], local[0])
]
agent.c.steer = steerPD(target[2], 0) * direction
agent.c.yaw = steerPD(target[2], -agent.me.rvel[2] / 4) * direction
agent.c.pitch = steerPD(target[1], agent.me.rvel[1] / 4) * direction
agent.c.roll = steerPD(target[0], agent.me.rvel[0] / 2.5)
return target
def push_shot(drone: CarObject, agent: MyHivemind):
left = Vector3(4200 * -agent.side(), agent.side() * 5120, 0)
right = Vector3(4200 * agent.side(), agent.side() * 5120, 0)
targets = {"goal": (agent.foe_goal.left_post, agent.foe_goal.right_post)}
if not agent.conceding:
drones = copy(agent.drones)
drones.remove(drone)
team = agent.friends + drones
for teammate in team:
a = teammate.location
b = teammate.location + 2000 * teammate.forward
local_a = drone.local(a)
angle_a = math.atan2(local_a.y, local_a.x)
if angle_a > 0:
targets["teammate" + str(team.index(teammate))] = (b, a)
else:
targets["teammate" + str(team.index(teammate))] = (a, b)
targets["upfield"] = (left, right)
shots = find_hits(drone, agent, targets)
if len(shots["goal"]) > 0:
drone.clear()
drone.push(shots["goal"][0])
drone.action = Action.Going
elif shots.get("teammate0") is not None and len(
shots.get("teammate0")) > 0:
drone.clear()
drone.push(shots["teammate0"][0])
drone.action = Action.Going
elif shots.get("teammate1") is not None and len(
shots.get("teammate1")) > 0:
drone.clear()
drone.push(shots["teammate1"][0])
drone.action = Action.Going
elif len(shots["upfield"]) > 0:
drone.clear()
drone.push(shots["upfield"][0])
drone.action = Action.Going
