def get_goto_point(self, bot, src, point): point = xy(point) desired_dir = self.get_center_dir() desired_dir_inv = -1 * desired_dir car_pos = xy(src) point_to_car = car_pos - point ang_to_desired_dir = angle_between(desired_dir_inv, point_to_car) ANG_ROUTE_ACCEPTED = math.pi / 4.3 can_go_straight = abs(ang_to_desired_dir) < self.span_size() / 2.0 can_with_route = abs( ang_to_desired_dir) < self.span_size() / 2.0 + ANG_ROUTE_ACCEPTED point = point + desired_dir_inv * 50 if can_go_straight: return point, 1.0 elif can_with_route: ang_to_right = abs(angle_between(point_to_car, -1 * self.right_dir)) ang_to_left = abs(angle_between(point_to_car, -1 * self.left_dir)) closest_dir = self.right_dir if ang_to_right < ang_to_left else self.left_dir goto = curve_from_arrival_dir(car_pos, point, closest_dir) goto.x = clip(goto.x, -Field.WIDTH / 2, Field.WIDTH / 2) goto.y = clip(goto.y, -Field.LENGTH / 2, Field.LENGTH / 2) draw.line(car_pos, goto, draw.color(150, 150, 150)) draw.line(point, goto, draw.color(150, 150, 150)) draw.bezier([car_pos, goto, point], draw.grey()) return goto, 0.5 else: return None, 1
def avoid_goal_post(self, bot, point): car = bot.info.my_car car_to_point = point - car.pos # Car is not in goal, not adjustment needed if abs(car.pos.y) < Field.LENGTH / 2: return # Car can go straight, not adjustment needed if car_to_point.x == 0: return # Do we need to cross a goal post to get to the point? goalx = Field.GOAL_WIDTH / 2 - 100 goaly = Field.LENGTH / 2 - 100 t = max((goalx - car.pos.x) / car_to_point.x, (-goalx - car.pos.x) / car_to_point.x) # This is the y coordinate when car would hit a goal wall. Is that inside the goal? crossing_goalx_at_y = abs(car.pos.y + t * car_to_point.y) if crossing_goalx_at_y > goaly: # Adjustment is needed point.x = clip(point.x, -goalx, goalx) point.y = clip(point.y, -goaly, goaly) if bot.do_rendering: bot.renderer.draw_line_3d(car.pos, point, bot.renderer.green())
def align(self, bot, target_rot: Mat33) -> SimpleControllerState: car = bot.info.my_car local_forward = dot(target_rot.col(0), car.rot) local_up = dot(target_rot.col(2), car.rot) local_ang_vel = dot(car.ang_vel, car.rot) pitch_ang = math.atan2(-local_forward.z, local_forward.x) pitch_ang_vel = local_ang_vel.y yaw_ang = math.atan2(-local_forward.y, local_forward.x) yaw_ang_vel = -local_ang_vel.z roll_ang = math.atan2(-local_up.y, local_up.z) roll_ang_vel = local_ang_vel.x forwards_dot = dot(target_rot.col(0), car.forward) roll_scale = forwards_dot**2 if forwards_dot > 0.85 else 0 self.controls.pitch = clip(-3.3 * pitch_ang + 0.8 * pitch_ang_vel, -1, 1) self.controls.yaw = clip(-3.3 * yaw_ang + 0.9 * yaw_ang_vel, -1, 1) self.controls.roll = clip(-3 * roll_ang + 0.5 * roll_ang_vel, -1, 1) * roll_scale self.controls.throttle = 1 return self.controls
def solve_PWL(a, b, c): xp = c / (a + b) if abs(a + b) > 10e-6 else -1 xm = c / (a - b) if abs(a - b) > 10e-6 else 1 if xm <= 0 <= xp: if abs(xp) < abs(xm): return clip(xp, 0, 1) else: return clip(xm, -1, 0) else: if 0 <= xp: return clip(xp, 0, 1) if xm <= 0: return clip(xm, -1, 0) return 0
def exec(self, bot): car = bot.info.my_car # For testing # f = normalize(bot.info.ball.pos - car.pos) # l = cross(f, Vec3(z=1)) # u = cross(l, f) # self.target = Mat33.from_columns(f, l, u) bot.renderer.draw_line_3d(car.pos, car.pos + 200 * self.target.col(0), bot.renderer.red()) bot.renderer.draw_line_3d(car.pos, car.pos + 200 * self.target.col(1), bot.renderer.green()) bot.renderer.draw_line_3d(car.pos, car.pos + 200 * self.target.col(2), bot.renderer.blue()) self.done |= car.on_ground local_forward = dot(self.target.col(0), car.rot) local_up = dot(self.target.col(2), car.rot) local_ang_vel = dot(car.ang_vel, car.rot) pitch_ang = math.atan2(local_forward.z, local_forward.x) pitch_ang_vel = local_ang_vel.y yaw_ang = math.atan2(-local_forward.y, local_forward.x) yaw_ang_vel = -local_ang_vel.z roll_ang = math.atan2(-local_up.y, local_up.z) roll_ang_vel = local_ang_vel.x P_pitch = 3.8 D_pitch = 0.8 P_yaw = -3.8 D_yaw = 0.9 P_roll = -3.3 D_roll = 0.5 uprightness = dot(car.up, self.target.col(2)) yaw_scale = 0.0 if uprightness < 0.6 else uprightness ** 2 return SimpleControllerState( pitch=clip(P_pitch * pitch_ang + D_pitch * pitch_ang_vel, -1, 1), yaw=clip((P_yaw * yaw_ang + D_yaw * yaw_ang_vel) * yaw_scale, -1, 1), roll=clip(P_roll * roll_ang + D_roll * roll_ang_vel, -1, 1), throttle=1.0, )
def get_boost_pad_convenience_score(self, pad): if not pad.is_active: return 0 car_to_pad = pad.pos - self.my_car.pos angle = angle_between(self.my_car.forward, car_to_pad) # Pads behind the car is bad if abs(angle) > 1.3: return 0 dist = norm(car_to_pad) dist_score = 1 - clip((abs(dist) / 2500)**2, 0, 1) angle_score = 1 - clip((abs(angle) / 3), 0, 1) return dist_score * angle_score * (0.8, 1)[pad.is_big]
def rotation_to_axis(rot: Mat33) -> Vec3: ang = math.acos(clip(0.5 * (tr(rot) - 1.0), -1.0, 1.0)) # For small angles, prefer series expansion to division by sin(theta) ~ 0 if abs(ang) < 0.00001: scale = 0.5 + ang * ang / 12.0 else: scale = 0.5 * ang / math.sin(ang) return Vec3( rot.get(2, 1) - rot.get(1, 2), rot.get(0, 2) - rot.get(2, 0), rot.get(1, 0) - rot.get(0, 1)) * scale
def curve_from_arrival_dir(src: Vec3, target: Vec3, arrival_direction: Vec3, w=1): """ Returns a point that is equally far from src and target on the line going through target with the given direction """ dir = normalize(arrival_direction) tx = target.x ty = target.y sx = src.x sy = src.y dx = dir.x dy = dir.y t = - (tx * tx - 2 * tx * sx + ty * ty - 2 * ty * sy + sx * sx + sy * sy) / (2 * (tx * dx + ty * dy - sx * dx - sy * dy)) t = clip(t, -1700, 1700) return target + w * t * dir
def is_viable(self, car, current_time: float): up = car.up T = self.intercept_time - current_time xf = car.pos + car.vel * T + 0.5 * GRAVITY * T ** 2 vf = car.vel + GRAVITY * T if self.jumping: if self.jump_begin_time == -1: jump_elapsed = 0 else: jump_elapsed = current_time - self.jump_begin_time # How much longer we can press jump and still gain upward force tau = JUMP_MAX_DUR - jump_elapsed # Add jump pulse if jump_elapsed == 0: vf += up * JUMP_SPEED xf += up * JUMP_SPEED * T # Acceleration from holding jump vf += up * JUMP_ACCEL * tau xf += up * JUMP_ACCEL * tau * (T - 0.5 * tau) if self.do_second_jump: # Impulse from the second jump vf += up * JUMP_SPEED xf += up * JUMP_SPEED * (T - tau) delta_x = self.hit_pos - xf dir = normalize(delta_x) phi = angle_between(dir, car.forward) turn_time = 0.7 * (2 * math.sqrt(phi / 9)) tau1 = turn_time * clip(1 - 0.3 / phi, 0.02, 1) required_acc = (2 * norm(delta_x)) / ((T - tau1) ** 2) ratio = required_acc / BOOST_ACCEL tau2 = T - (T - tau1) * math.sqrt(1 - clip01(ratio)) velocity_estimate = vf + BOOST_ACCEL * (tau2 - tau1) * dir boost_estimate = (tau2 - tau1) * BOOST_PR_SEC enough_boost = boost_estimate < 0.95 * car.boost enough_time = abs(ratio) < 0.9 return norm(velocity_estimate) < 0.9 * MAX_SPEED and enough_boost and enough_time
def run(self, bot) -> SimpleControllerState: car = bot.info.my_car ball = bot.info.ball hits_goal_prediction = predict.will_ball_hit_goal(bot) reach_time = clip(predict.time_till_reach_ball(car, ball), 0, hits_goal_prediction.time - 0.5) reachable_ball = predict.ball_predict(bot, reach_time) self.ball_to_goal_right = bot.info.own_goal.right_post - reachable_ball.pos self.ball_to_goal_left = bot.info.own_goal.left_post - reachable_ball.pos self.aim_cone = AimCone(self.ball_to_goal_left, self.ball_to_goal_right) self.aim_cone.draw(reachable_ball.pos, r=200, g=0, b=160) shoot_controls = bot.shoot.with_aiming(bot, self.aim_cone, reach_time) if not bot.shoot.can_shoot: # Go home return bot.drive.home(bot) else: return shoot_controls
def towards_point(self, bot, point: Vec3, target_vel=1430, slide=False, boost_min=101, can_keep_speed=True, can_dodge=True, wall_offset_allowed=125) -> SimpleControllerState: REQUIRED_ANG_FOR_SLIDE = 1.65 REQUIRED_VELF_FOR_DODGE = 1100 car = bot.info.my_car # Dodge is done if self.dodge is not None and self.dodge.done: self.dodge = None self.last_dodge_end_time = bot.info.time # Continue dodge elif self.dodge is not None: self.dodge.target = point return self.dodge.exec(bot) # Begin recovery if not car.on_ground: bot.maneuver = RecoveryManeuver() return self.controls # Get down from wall by choosing a point close to ground if not is_near_wall(point, wall_offset_allowed) and angle_between( car.up, Vec3(0, 0, 1)) > math.pi * 0.31: point = lerp(xy(car.pos), xy(point), 0.5) # If the car is in a goal, avoid goal posts self._avoid_goal_post(bot, point) car_to_point = point - car.pos # The vector from the car to the point in local coordinates: # point_local.x: how far in front of my car # point_local.y: how far to the left of my car # point_local.z: how far above my car point_local = dot(point - car.pos, car.rot) # Angle to point in local xy plane and other stuff angle = math.atan2(point_local.y, point_local.x) dist = norm(point_local) vel_f = proj_onto_size(car.vel, car.forward) vel_towards_point = proj_onto_size(car.vel, car_to_point) # Start dodge if can_dodge and abs(angle) <= 0.02 and vel_towards_point > REQUIRED_VELF_FOR_DODGE\ and dist > vel_towards_point + 500 + 900 and bot.info.time > self.last_dodge_end_time + self.dodge_cooldown: self.dodge = DodgeManeuver(bot, point) # Start half-flip elif can_dodge and abs(angle) >= 3 and vel_towards_point < 0\ and dist > -vel_towards_point + 500 + 900 and bot.info.time > self.last_dodge_end_time + self.dodge_cooldown: self.dodge = HalfFlipManeuver(bot, boost=car.boost > boost_min + 10) # Is point right behind? Maybe reverse instead if -100 < point_local.x < 0 and abs(point_local.y) < 50: #bot.print("Reversing?") pass # Is in turn radius deadzone? tr = turn_radius(abs(vel_f + 50)) # small bias tr_side = sign(angle) tr_center_local = Vec3(0, tr * tr_side, 10) point_is_in_turn_radius_deadzone = norm(point_local - tr_center_local) < tr # Draw turn radius deadzone if car.on_ground and False: tr_center_world = car.pos + dot(car.rot, tr_center_local) tr_center_world_2 = car.pos + dot(car.rot, -1 * tr_center_local) color = draw.orange() draw.circle(tr_center_world, car.up, tr, color) draw.circle(tr_center_world_2, car.up, tr, color) if point_is_in_turn_radius_deadzone: # Hard turn self.controls.steer = sign(angle) self.controls.boost = False self.controls.throttle = 0 if vel_f > 150 else 0.1 if point_local.x < 110 and point_local.y < 400 and norm( car.vel) < 300: # Brake or go backwards when the point is really close but not in front of us self.controls.throttle = clip(-0.25 + point_local.x / -110.0, 0, -1) self.controls.steer = -0.5 * sign(angle) else: # Should drop speed or just keep up the speed? if can_keep_speed and target_vel < vel_towards_point: target_vel = vel_towards_point else: # Small lerp adjustment target_vel = lerp(vel_towards_point, target_vel, 1.1) # Turn and maybe slide self.controls.steer = clip(angle + (2.5 * angle)**3, -1.0, 1.0) if slide and abs(angle) > REQUIRED_ANG_FOR_SLIDE: self.controls.handbrake = True self.controls.steer = sign(angle) else: self.controls.handbrake = False # Overshoot target vel for quick adjustment target_vel = lerp(vel_towards_point, target_vel, 1.2) # Find appropriate throttle/boost if vel_towards_point < target_vel: self.controls.throttle = 1 if boost_min < car.boost and vel_towards_point + 80 < target_vel and target_vel > 1400 \ and not self.controls.handbrake and is_heading_towards(angle, dist): self.controls.boost = True else: self.controls.boost = False else: vel_delta = target_vel - vel_towards_point self.controls.throttle = clip(0.2 + vel_delta / 500, 0, -1) self.controls.boost = False if self.controls.handbrake: self.controls.throttle = min(0.4, self.controls.throttle) # Saved if something outside calls start_dodge() in the meantime self.last_point = point return self.controls
def with_aiming(self, bot, aim_cone: AimCone, time: float, dodge_hit: bool = True): # aim: | | | | # ball | bad | ok | good | # z pos: | | | | # -----------+-----------+-----------+-----------+ # too high | give | give | wait/ | # > 1200 | up | up | improve | # -----------+ - - - - - + - - - - - + - - - - - + # medium | give | improve | aerial | # | up | aim | | # -----------+ - - - - - + - - - - - + - - - - - + # soon on | improve | slow | small | # ground | aim | curve | jump | # -----------+ - - - - - + - - - - - + - - - - - + # on ground | improve | fast | fast | # | aim?? | curve | straight | # -----------+ - - - - - + - - - - - + - - - - - + # FIXME if the ball is not on the ground we treat it as 'soon on ground' in all other cases self.controls = SimpleControllerState() self.aim_is_ok = False self.waits_for_fall = False self.ball_is_flying = False self.can_shoot = False self.using_curve = False self.curve_point = None car = bot.info.my_car ball_soon = ball_predict(bot, time) car_to_ball_soon = ball_soon.pos - car.pos dot_facing_score = dot(normalize(car_to_ball_soon), normalize(car.forward)) dot_facing_score_2d = dot(normalize(xy(car_to_ball_soon)), normalize(xy(car.forward))) vel_towards_ball_soon = proj_onto_size(car.vel, car_to_ball_soon) is_facing = 0.1 < dot_facing_score is_facing_2d = 0.3 < dot_facing_score self.ball_when_hit = ball_soon if ball_soon.pos.z < 110: # The ball is on the ground if 110 < ball_soon.pos.z: # and ball_soon.vel.z <= 0: # The ball is slightly in the air, lets wait just a bit more self.waits_for_fall = True ball_landing = next_ball_landing(bot, ball_soon, size=100) time = time + ball_landing.time ball_soon = ball_predict(bot, time) car_to_ball_soon = ball_soon.pos - car.pos self.ball_when_hit = ball_soon # The ball is on the ground, are we in position for a shot? if aim_cone.contains_direction(car_to_ball_soon) and is_facing: # Straight shot self.aim_is_ok = True self.can_shoot = True if norm(car_to_ball_soon) < 400 + Ball.RADIUS and aim_cone.contains_direction(car_to_ball_soon)\ and vel_towards_ball_soon > 300: bot.drive.start_dodge(bot, towards_ball=True) offset_point = xy(ball_soon.pos) - 50 * aim_cone.get_center_dir() speed = self._determine_speed(norm(car_to_ball_soon), time) self.controls = bot.drive.towards_point(bot, offset_point, target_vel=speed, slide=True, boost_min=0, can_keep_speed=False) return self.controls elif aim_cone.contains_direction(car_to_ball_soon, math.pi / 5): # Curve shot self.aim_is_ok = True self.using_curve = True self.can_shoot = True offset_point = xy(ball_soon.pos) - 50 * aim_cone.get_center_dir() closest_dir = aim_cone.get_closest_dir_in_cone(car_to_ball_soon) self.curve_point = curve_from_arrival_dir(car.pos, offset_point, closest_dir) self.curve_point.x = clip(self.curve_point.x, -Field.WIDTH / 2, Field.WIDTH / 2) self.curve_point.y = clip(self.curve_point.y, -Field.LENGTH / 2, Field.LENGTH / 2) if dodge_hit and norm(car_to_ball_soon) < 400 + Ball.RADIUS and angle_between(car.forward, car_to_ball_soon) < 0.5\ and aim_cone.contains_direction(car_to_ball_soon) and vel_towards_ball_soon > 300: bot.drive.start_dodge(bot, towards_ball=True) speed = self._determine_speed(norm(car_to_ball_soon), time) self.controls = bot.drive.towards_point(bot, self.curve_point, target_vel=speed, slide=True, boost_min=0, can_keep_speed=False) return self.controls else: # We are NOT in position! return None elif ball_soon.pos.z < 600 and ball_soon.vel.z <= 0: # Ball is on ground soon. Is it worth waiting? TODO if aim is bad, do a slow curve - or delete case? pass # --------------------------------------- # Ball is in the air, or going in the air if 200 < ball_soon.pos.z < 1400 and aim_cone.contains_direction(car_to_ball_soon) and is_facing_2d: # Can we hit it if we make jump shot or aerial shot? vel_f = proj_onto_size(car.vel, xy(car_to_ball_soon)) aerial = ball_soon.pos.z > 750 if vel_f > 400: # Some forward momentum is required flat_dist = norm(xy(car_to_ball_soon)) # This range should be good https://www.desmos.com/calculator/bx9imtiqi5 good_height = 0.3 * ball_soon.pos.z < flat_dist < 4 * ball_soon.pos.z if good_height: # Alternative ball positions alternatives = [ (ball_predict(bot, time * 0.8), time * 0.8), (ball_predict(bot, time * 0.9), time * 0.9), (ball_soon, time), (ball_predict(bot, time * 1.1), time * 1.1), (ball_predict(bot, time * 1.2), time * 1.2) ] for alt_ball, alt_time in alternatives: potential_small_jump_shot = JumpShotManeuver(bot, alt_ball.pos, bot.info.time + alt_time, do_second_jump=aerial) jump_shot_viable = potential_small_jump_shot.is_viable(car, bot.info.time) if jump_shot_viable: self.can_shoot = True self.aim_is_ok = True bot.maneuver = potential_small_jump_shot return bot.maneuver.exec(bot) self.ball_is_flying = True return self.controls
def ball_predict(bot, time: float) -> DummyObject: """ Returns a DummyObject describing the expected position and velocity of the ball """ path = bot.get_ball_prediction_struct() t = int(clip(360 * time / 6, 1, path.num_slices)) - 1 return DummyObject(path.slices[t].physics)
def with_aiming(self, bot, aim_cone: AimCone, time: float, dodge_hit: bool = True): # aim: | | | | # ball | bad | ok | good | # z pos: | | | | # -----------+-----------+-----------+-----------+ # too high | give | give | wait/ | # | up | up | improve | # -----------+ - - - - - + - - - - - + - - - - - + # medium | give | improve | aerial | # | up | aim | | # -----------+ - - - - - + - - - - - + - - - - - + # soon on | improve | slow | small | # ground | aim | curve | jump | # -----------+ - - - - - + - - - - - + - - - - - + # on ground | improve | fast | fast | # | aim?? | curve | straight | # -----------+ - - - - - + - - - - - + - - - - - + # FIXME if the ball is not on the ground we treat it as 'soon on ground' in all other cases self.controls = SimpleControllerState() self.aim_is_ok = False self.waits_for_fall = False self.ball_is_flying = False self.can_shoot = False self.using_curve = False self.curve_point = None self.ball_when_hit = None car = bot.info.my_car ball_soon = ball_predict(bot, time) car_to_ball_soon = ball_soon.pos - car.pos dot_facing_score = dot(normalize(car_to_ball_soon), normalize(car.forward)) vel_towards_ball_soon = proj_onto_size(car.vel, car_to_ball_soon) is_facing = 0 < dot_facing_score if ball_soon.pos.z < 110 or (ball_soon.pos.z < 475 and ball_soon.vel.z <= 0) or True: #FIXME Always true # The ball is on the ground or soon on the ground if 275 < ball_soon.pos.z < 475 and aim_cone.contains_direction( car_to_ball_soon): # Can we hit it if we make a small jump? vel_f = proj_onto_size(car.vel, xy(car_to_ball_soon)) car_expected_pos = car.pos + car.vel * time ball_soon_flat = xy(ball_soon.pos) diff = norm(car_expected_pos - ball_soon_flat) ball_in_front = dot(ball_soon.pos - car_expected_pos, car.vel) > 0 if bot.do_rendering: bot.renderer.draw_line_3d(car.pos, car_expected_pos, bot.renderer.lime()) bot.renderer.draw_rect_3d(car_expected_pos, 12, 12, True, bot.renderer.lime()) if vel_f > 400: if diff < 150 and ball_in_front: bot.maneuver = SmallJumpManeuver( bot, lambda b: b.info.ball.pos) if 110 < ball_soon.pos.z: # and ball_soon.vel.z <= 0: # The ball is slightly in the air, lets wait just a bit more self.waits_for_fall = True ball_landing = next_ball_landing(bot, ball_soon, size=100) time = time + ball_landing.time ball_soon = ball_predict(bot, time) car_to_ball_soon = ball_soon.pos - car.pos self.ball_when_hit = ball_soon # The ball is on the ground, are we in position for a shot? if aim_cone.contains_direction(car_to_ball_soon) and is_facing: # Straight shot self.aim_is_ok = True self.can_shoot = True if norm(car_to_ball_soon) < 240 + Ball.RADIUS and aim_cone.contains_direction(car_to_ball_soon)\ and vel_towards_ball_soon > 300: bot.drive.start_dodge(bot) offset_point = xy( ball_soon.pos) - 50 * aim_cone.get_center_dir() speed = self.determine_speed(norm(car_to_ball_soon), time) self.controls = bot.drive.go_towards_point( bot, offset_point, target_vel=speed, slide=True, boost_min=0, can_keep_speed=False) return self.controls elif aim_cone.contains_direction(car_to_ball_soon, math.pi / 5): # Curve shot self.aim_is_ok = True self.using_curve = True self.can_shoot = True offset_point = xy( ball_soon.pos) - 50 * aim_cone.get_center_dir() closest_dir = aim_cone.get_closest_dir_in_cone( car_to_ball_soon) self.curve_point = curve_from_arrival_dir( car.pos, offset_point, closest_dir) self.curve_point.x = clip(self.curve_point.x, -Field.WIDTH / 2, Field.WIDTH / 2) self.curve_point.y = clip(self.curve_point.y, -Field.LENGTH / 2, Field.LENGTH / 2) if dodge_hit and norm(car_to_ball_soon) < 240 + Ball.RADIUS and angle_between(car.forward, car_to_ball_soon) < 0.5\ and aim_cone.contains_direction(car_to_ball_soon) and vel_towards_ball_soon > 300: bot.drive.start_dodge(bot) speed = self.determine_speed(norm(car_to_ball_soon), time) self.controls = bot.drive.go_towards_point( bot, self.curve_point, target_vel=speed, slide=True, boost_min=0, can_keep_speed=False) return self.controls else: # We are NOT in position! self.aim_is_ok = False pass else: if aim_cone.contains_direction(car_to_ball_soon): self.waits_for_fall = True self.aim_is_ok = True #self.can_shoot = False pass # Allow small aerial (wait if ball is too high) elif aim_cone.contains_direction(car_to_ball_soon, math.pi / 4): self.ball_is_flying = True pass # Aim is ok, but ball is in the air