Determine a torpedo firing solution (launch bearing) - mostly for use in playing submarine simulation video games :)

firing_solution.py

#! /usr/bin/env python3

"""
usage: firing_solution bearing range course speed torpedo_speed [max_range]

Determine a firing solution (launch bearing).

positional arguments:
  bearing        compass bearing to target (degrees)
  range          range to target (nm)
  course         target's course (degrees)
  speed          target's speed (kt)
  torpedo_speed  torpedo's speed (kt)
  max_range      (optional) maximum torpedo range (nm)

options:
  -h, --help     show this help message and exit
"""

import argparse
import math

EPSILON = 1e-8

class InterceptError(Exception):
    """Exception class to report impossible intercepts."""
    pass

def parse_args():
    parser = argparse.ArgumentParser(prefix_chars=r'/-@',
    description='Determine a firing solution (launch bearing).')

    parser.add_argument('bearing', type=float,
                        help='compass bearing to target (degrees)')
    parser.add_argument('range', type=float,
                        help='range to target (nm)')
    parser.add_argument('course', type=float,
                        help="target's course (degrees)")
    parser.add_argument('speed', type=float,
                        help="target's speed (kt)")
    parser.add_argument('torpedo_speed', type=float,
                        help="torpedo's speed (kt)")
    parser.add_argument('max_range', type=float, nargs = '?', default=None,
                        help='(optional) maximum torpedo range (nm)')

    args = parser.parse_args()
    return args

def bearing_to_angle(bearing):
    """Convert a compass bearing to a Cartesian angle.
    
    Argument:
    bearing -- in degrees measured clockwise from North
    
    Returns:
    angle -- the equivalent angle in radians
             measured CCW from the positive x axis
    """
    return math.radians((90 - bearing) % 360)

def angle_to_bearing(angle):
    """Convert a Cartesian angle to a compass bearing.
    
    Argument:
    angle -- the equivalent angle in radians
             measured CCW from the positive x axis
    
    Returns:
    bearing -- in degrees measured clockwise from North
    """
    return (90 - math.degrees(angle)) % 360

def time_string(hours):
    """Reformat hours to an 'hh:mm:ss' string for display"""
    if hours == 0:
        raise ValueError('hours value may not be zero')
    hh = int(hours)
    mm = int((hours - hh) * 60)
    ss = int((hours - hh - mm / 60) * 3600)
    if hh == 0:
        hh = None
    if mm == 0 and hh is None:
        mm = None
    time_string = ':'.join(f'{elem:02}' for elem in (hh, mm, ss)
                           if elem is not None)
    if time_string[0] == '0':
        time_string = time_string[1:]
    if ':' not in time_string:
        time_string += ' seconds'
    return time_string

def find_solution(target_bearing, target_range,
                  target_course, target_speed,
                  torpedo_speed, torpedo_max_range=None):
    """Determine a firing solution (launch bearing).
    
    Arguments:
    target_bearing -- bearing to the target (compass bearing)
    target_range -- range to the target
    target_course -- target's course (compass bearing)
    target_speed -- target's speed
    torpedo_speed -- speed of the torpedo
    torpedo_max_range -- (optional) maximum range of the torpedo
    
    Returns:
    launch_bearing -- the launch bearing (compass bearing)
    run_time -- the time to reach the intercept point
    intercept_distance -- the distance to the intercept point
    
    Exceptions:
    Raise an InterceptError with appropriate message if no intercept is
    possible.
    If the optional torpedo_max_range is given raise an InterceptError
    if the intercept distance exceeds the max range.
    
    Note regarding units:
    All bearings and courses must be given as compass bearings
    - measured in degrees measured clockwise from North.
    Speeds and ranges are unit-agnostic, but do require the units to be
    consistent.
    The return values are in the same units as the input values, and
    time is derived from the time unit of the speed input (hours if
    in kph, mph, or kt; seconds if in m/s or feet per second).
    It is left to the caller to provide any unit conversions required.
    """
    # Convert from compass bearings to Cartesian angles
    target_bearing = bearing_to_angle(target_bearing)
    target_course = bearing_to_angle(target_course)
    
    # Convert from radial to Cartesian coordinates and vectors
    target_x = target_range * math.cos(target_bearing)
    target_y = target_range * math.sin(target_bearing)

    target_vel_x = target_speed * math.cos(target_course)
    target_vel_y = target_speed * math.sin(target_course)

    # Quadratic coefficients
    a = target_vel_x**2 + target_vel_y**2 - torpedo_speed**2
    b = 2 * (target_x*target_vel_x + target_y*target_vel_y)
    c = target_x**2 + target_y**2

    if abs(a) < EPSILON: # Speeds match so switch to linear solve
        if abs(b) < EPSILON: # Slope == 0, parallel motion with no intercept
            raise InterceptError('No solution found - parallel motion')
        else:
            run_time = -c / b
            if run_time < 0: # No solution in the future
                raise InterceptError('No future solution found')
    else: # Quadratic solve
        # Discriminant
        disc = b**2 - 4*a*c

        if disc < 0: # No real solutions
            raise InterceptError('No solution found')
        
        t1 = (-b + math.sqrt(disc)) / (2 * a)
        t2 = (-b - math.sqrt(disc)) / (2 * a)
        
        run_time = min((ti for ti in (t1, t2) if ti > 0), default=None)
        if run_time is None: # Only negative solutions
            raise InterceptError('No future solution found')
    
    intercept_x = target_x + target_vel_x*run_time
    intercept_y = target_y + target_vel_y*run_time
    intercept_distance = math.sqrt(intercept_x**2 + intercept_y**2)
    
    if (torpedo_max_range is not None
        and intercept_distance > torpedo_max_range):
        raise InterceptError('Intercept distance exceeds weapon range')
    
    launch_angle = math.atan2(intercept_y, intercept_x)
    launch_bearing = round(angle_to_bearing(launch_angle), 1)
    
    return launch_bearing, run_time, intercept_distance

if __name__ == '__main__':
    args = parse_args()
    try:
        launch_bearing, run_time, intercept_distance = find_solution(
            target_bearing=args.bearing,
            target_range=args.range,
            target_course=args.course,
            target_speed=args.speed,
            torpedo_speed=args.torpedo_speed,
            torpedo_max_range=args.max_range,
            )
    except InterceptError as err:
        print(err)
    else:    
        print(f"""Solution found:

Bearing : {launch_bearing}°
Distance: {intercept_distance:,.2f}
Run Time: {time_string(run_time)}
""")