1. import math
2.  
3.  
4. def dist(point1, point2):
5.     return ((point1[0] - point2[0]) ** 2 + (point1[1] - point2[1]) ** 2) ** 0.5
6.  
7.  
8. # thanks to https://stackoverflow.com/questions/20924085/python-conversion-between-coordinates
9. def rect(r, theta):
10.     """
11.     theta in degrees
12.  
13.     returns tuple; (float, float); (x,y)
14.     """
15.  
16.     x = r * math.cos(math.radians(theta))
17.     y = r * math.sin(math.radians(theta))
18.     return x, y
19.  
20.  
21. # thanks to https://stackoverflow.com/questions/20924085/python-conversion-between-coordinates
22. def polar(x, y):
23.     """
24.     returns r, theta(degrees)
25.     """
26.  
27.     r = (x ** 2 + y ** 2) ** .5
28.     theta = math.degrees(math.atan2(y,x))
29.     return r, theta
30.  
31.  
32. def angle_mod_360(angle):
33.     """
34.     Maps an angle to the interval -180, +180.
35.  
36.     Examples:
37.     angle_mod_360(362) == 2
38.     angle_mod_360(270) == -90
39.  
40.     :param angle: angle in degree
41.     :return: angle in degree. Between -180 and +180
42.     """
43.  
44.     n = math.floor(angle/360.0)
45.  
46.     angle_between_0_and_360 = angle - n*360.0
47.  
48.     if angle_between_0_and_360 <= 180.0:
49.         return angle_between_0_and_360
50.     else:
51.         return angle_between_0_and_360 - 360
52.  
53.  
54. def get_waypoints_ordered_in_driving_direction(params):
55.     # waypoints are always provided in counter clock wise order
56.     if params['is_reversed']: # driving clock wise.
57.         return list(reversed(params['waypoints']))
58.     else: # driving counter clock wise.
59.         return params['waypoints']
60.  
61.  
62. def up_sample(waypoints, factor):
63.     """
64.     Adds extra waypoints in between provided waypoints
65.  
66.     :param waypoints:
67.     :param factor: integer. E.g. 3 means that the resulting list has 3 times as many points.
68.     :return:
69.     """
70.     p = waypoints
71.     n = len(p)
72.  
73.     return [[i / factor * p[(j+1) % n][0] + (1 - i / factor) * p[j][0],
74.              i / factor * p[(j+1) % n][1] + (1 - i / factor) * p[j][1]] for j in range(n) for i in range(factor)]
75.  
76.  
77. def get_target_point(params):
78.     waypoints = up_sample(get_waypoints_ordered_in_driving_direction(params), 20)
79.  
80.     car = [params['x'], params['y']]
81.  
82.     distances = [dist(p, car) for p in waypoints]
83.     min_dist = min(distances)
84.     i_closest = distances.index(min_dist)
85.  
86.     n = len(waypoints)
87.  
88.     waypoints_starting_with_closest = [waypoints[(i+i_closest) % n] for i in range(n)]
89.  
90.     r = params['track_width'] * 0.9
91.  
92.     is_inside = [dist(p, car) < r for p in waypoints_starting_with_closest]
93.     i_first_outside = is_inside.index(False)
94.  
95.     if i_first_outside < 0:  # this can only happen if we choose r as big as the entire track
96.         return waypoints[i_closest]
97.  
98.     return waypoints_starting_with_closest[i_first_outside]
99.  
100.  
101. def get_target_steering_degree(params):
102.     tx, ty = get_target_point(params)
103.     car_x = params['x']
104.     car_y = params['y']
105.     dx = tx-car_x
106.     dy = ty-car_y
107.     heading = params['heading']
108.  
109.     _, target_angle = polar(dx, dy)
110.  
111.     steering_angle = target_angle - heading
112.  
113.     return angle_mod_360(steering_angle)
114.  
115.  
116. def score_steer_to_point_ahead(params):
117.     best_stearing_angle = get_target_steering_degree(params)
118.     steering_angle = params['steering_angle']
119.  
120.     error = (steering_angle - best_stearing_angle) / 60.0  # 60 degree is already really bad
121.  
122.     score = 1.0 - abs(error)
123.  
124.     return max(score, 0.01)  # optimizer is rumored to struggle with negative numbers and numbers too close to zero
125.  
126.  
127. def reward_function(params):
128.     return float(score_steer_to_point_ahead(params))