-
Notifications
You must be signed in to change notification settings - Fork 3
/
utils.py
134 lines (119 loc) · 6.13 KB
/
utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
import math
from objects import Vector3
#This file is for small utilities for math and movement
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 cap(x, low, high):
#caps/clamps a number between a low and high value
if x < low:
return low
elif x > high:
return high
return x
def defaultPD(agent, local_target, direction = 1.0):
#points the car towards a given local target.
#Direction can be changed to allow the car to steer towards a target while driving backwards
local_target *= direction
up = agent.me.local(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 determing the controller inputs
agent.controller.steer = steerPD(target_angles[1],0) * direction
agent.controller.pitch = steerPD(target_angles[0],agent.me.angular_velocity[1]/4)
agent.controller.yaw = steerPD(target_angles[1],-agent.me.angular_velocity[2]/4)
agent.controller.roll = steerPD(target_angles[2],agent.me.angular_velocity[0]/2)
#Returns the angles, which can be useful for other purposes
return target_angles
def defaultThrottle(agent, target_speed, direction = 1.0):
#accelerates the car to a desired speed using throttle and boost
car_speed = agent.me.local(agent.me.velocity)[0]
t = (target_speed * direction) - car_speed
agent.controller.throttle = cap((t**2) * sign(t)/1000, -1.0, 1.0)
agent.controller.boost = True if t > 150 and car_speed < 2275 and agent.controller.throttle == 1.0 else False
return car_speed
def in_field(point,radius):
#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 find_slope(shot_vector,car_to_target):
#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 post_correction(ball_location, left_target, right_target):
#this function returns target locations that are corrected to account for the ball's radius
# it also checks to make sure the ball can fit between the corrected locations
ball_radius = 100 # We purposely make this larger so that our shots have a higher chance of success
goal_line_perp = (right_target - left_target).cross((0,0,1))
left_adjusted = left_target + ((left_target - ball_location).normalize().cross((0,0,-1))*ball_radius)
right_adjusted = right_target + ((right_target - ball_location).normalize().cross((0,0,1))*ball_radius)
left_corrected = left_target if (left_adjusted-left_target).dot(goal_line_perp) > 0.0 else left_adjusted
right_corrected = right_target if (right_adjusted-right_target).dot(goal_line_perp) > 0.0 else right_adjusted
new_goal_line, new_goal_width = (right_corrected - left_corrected).normalize(True)
new_goal_perp = (new_goal_line.cross((0, 0, 1)))
goal_center = left_corrected + (new_goal_line * new_goal_width * 0.5)
ball_to_goal = (goal_center - ball_location).normalize()
ball_fits = new_goal_width * abs(new_goal_perp.dot(ball_to_goal)) > ball_radius * 2
return left_corrected, right_corrected, goal_center, ball_fits
def quadratic(a,b,c):
#Returns the two roots of a quadratic
inside = math.sqrt((b*b) - (4*a*c))
if a != 0:
return (-b + inside)/(2*a),(-b - inside)/(2*a)
else:
return -1,-1
def shot_valid(agent, shot, threshold = 45):
#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 side(x):
#returns -1 for blue team and 1 for orange team
if x == 0:
return -1
return 1
def sign(x):
#returns the sign of a number, -1, 0, +1
if x < 0.0:
return -1
elif x > 0.0:
return 1
else:
return 0.0
def steerPD(angle, rate):
#A Proportional-Derivative control loop used for defaultPD
return cap(((35*(angle+rate))**3)/10, -1.0, 1.0)