def estimate_safe_speed_at_angle(true_radius, max_angle):
def check_speed(v):
#M_C(v, r)
#M_p(v, alpha)
#M_d(omega)
def my_M(v, r, alpha, omega):
return estimate_M_c(v, r) + M_p(v, alpha) + M_d(omega)
alpha = 0
omega = my_M(v, true_radius, alpha, 0.0)
it = 0
while omega > 0.0 and it < 100:
alpha += omega
omega += my_M(v, true_radius, alpha, omega)
if alpha > max_angle:
return False
it += 1
return True
v_max = max_velocity()
ret = my_bisect(v_max / 10, v_max, 7, check_speed)
#print("estimated safe speed for {0} to be {1}".format(true_radius, ret))
#print("whereas stable speed at the same angle is {0}".format(estimate_stable_speed_at_angle(true_radius, max_angle)))
return ret
def estimate_stable_speed_at_angle(true_radius, max_angle):
"""
M_p = M_c
"""
def ret(v):
return abs(M_p(v, max_angle)) >= estimate_M_c(v, true_radius)
v_max = max_velocity()
return my_bisect(v_max / 10, v_max, 7, ret)
def estimate_optimal_speed_at_bend_with_annealing(input_car, until_track_piece, overwrite_car_speed=False):
max_v = max_velocity()
min_v = safe_speed
def do_simulate_with_speed(v):
car = copy.copy(input_car)
if overwrite_car_speed:
car.velocity = v
while car.track_piece_index != until_track_piece:
t = throttle_to_reach_velocity(car.velocity, v)
step(car, t)
if not is_safe_state(car):
return False
# we're out of the bend, now annealing
while car.velocity > safe_speed:
step(car, 0.0)
if not is_safe_state(car):
return False
return True
return my_bisect(min_v, max_v, 7, do_simulate_with_speed) #FIXME tune these parameters, add timeout?
