Interception of objects with linear motion

# PYTHON 2.7

# examples:
def test():
    a = {
        'p': (-1.0,4.0),
        'v': (0.0,-2.0),
    }

    b = {
        'p': (10.0, 8.0),
        'v': (-5.0, 5.0),
    }

    print InterceptRelative(a,b,1)
    print InterceptRelative(a,b,10)
    print InterceptRelative(a,b,100)

    print InterceptAbsolute(a,b,1)
    print InterceptAbsolute(a,b,10)
    print InterceptAbsolute(a,b,100)

# suppose source will shoot at target
# the projectile's speed relative to source will equal speed
def InterceptRelative(source,target,speed):
    # we will work in 'source-relative-space'
    # it is as if source is stationary at the origin
    # and we just observe target
    delta_position = Vsub(target['p'], source['p']) # t-s gives the vector FROM s TO t
    delta_velocity = Vsub(target['v'], source['v'])

    return Intercept(delta_position, delta_velocity, speed)

# suppose source shoots at target
# but the projectile's speed relative to the universe equal's speed
def InterceptAbsolute(source,target,speed):
    # we will work in 'source-relative-space'
    # BUT we pretend source's velocity equals zero
    delta_position = Vsub(target['p'], source['p'])
    delta_velocity = target['v']

    return Intercept(delta_position, delta_velocity, speed)

# the actual interception function
# there is a sphere (circle in 2D) at the origin expanding at rate speed
# when does it touch the target?
def Intercept(target_pos, target_vel, speed):
    # there's some math going on here
    # roughly speaking, we imagine a sphere expanding from the origin at rate speed
    # and compute when that sphere will intersect the point
    a = Vsqmag(target_vel) - (speed**2.0)
    b = 2.0*Vdot(target_pos, target_vel)
    c = Vsqmag(target_pos)

    # use our solver
    times = Quadratic(a,b,c)

    if times is None:
        # there are no real solutions - therefore it's not possible to hit the target
        return None

    # choose the non-negative times
    times = [t for t in times if t >= 0]

    if len(times) == 0:
        # we can only hit it in the past! - so not really at all :(
        return None

    # choose the soonest time when we can hit it
    t = min(times)
    collisionpt = Vadd(target_pos, Vscale(target_vel, t))
    print t, collisionpt
    return Vscale(Vnormalize(collisionpt), speed)


# some vector operations
# note that they are dimensionless!
from itertools import izip
from math import sqrt, fsum

def Vadd(v1, v2):
    return tuple(x+y for (x,y) in izip(v1,v2))

def Vsub(v1, v2):
    return tuple(x-y for (x,y) in izip(v1,v2))

def Vscale(v, s):
    return tuple(x*s for x in v)

def Vdot(v1,v2):
    return fsum(x*y for (x,y) in izip(v1,v2))

def Vsqmag(v):
    return Vdot(v,v)

def Vmag(v):
    return sqrt(Vsqmag(v))

def Vnormalize(v):
    return Vscale(v, 1.0/Vmag(v))

# a simple quadratic solver for the form
# Ax^2 + Bx + C = 0
# returns results as a tuple (low x, high x)
# returns None if the result is complex
def Quadratic(A,B,C):
    if A == 0.0:
        # this is not quadratic
        if B == 0.0:
            # this is not even linear!
            return None
        else:
            return (-C/B,)

    radical = B*B - 4.0*A*C
    if(radical < 0):
        return None

    part1 = -B / (2.0*A)
    part2 = sqrt(radical)/(2.0*A)

    return (part1 - part2, part1 + part2)

if(__name__ == "__main__"):
    test()