view TTC Sample/timeToCollision.py @ 462:af2222c0c9c0

TTC tested and updatet!
author Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
date Mon, 03 Feb 2014 15:41:57 -0500
parents
children
line wrap: on
line source

def timeToCollision(obj1,obj2,collisionDistanceThreshold,frameNum,framePerSecond):

    import numpy as np

    x1 = obj1.getPositionAtInstant(frameNum).x
    y1 = obj1.getPositionAtInstant(frameNum).y
    x2 = obj2.getPositionAtInstant(frameNum).x
    y2 = obj2.getPositionAtInstant(frameNum).y
    v1x = obj1.getVelocityAtInstant(frameNum).x * framePerSecond
    v1y = obj1.getVelocityAtInstant(frameNum).y * framePerSecond
    v2x = obj2.getVelocityAtInstant(frameNum).x * framePerSecond
    v2y = obj2.getVelocityAtInstant(frameNum).y * framePerSecond
    l = collisionDistanceThreshold

    a = pow(v1x-v2x,2) + pow(v1y-v2y,2)
    b = 2 * ((x1-x2) * (v1x-v2x) + (y1-y2) * (v1y-v2y))
    c = pow(x1-x2,2) + pow(y1-y2,2) - pow(l,2)
    
    if pow(b,2) >= 4*a*c:
        ttc1 = (-b + np.sqrt(pow(b,2) - 4*a*c)) / (2*a)
        ttc2 = (-b - np.sqrt(pow(b,2) - 4*a*c)) / (2*a)
        if ttc1 >= 0 and ttc2 >= 0:
            ttc = min(ttc1,ttc2)
        else:
            if ttc1 < 0:
                ttc = ttc2
            if ttc2 < 0:
                ttc = ttc1
            if ttc1 < 0 and ttc2 < 0:
                ttc = []
    else:
        ttc = []

    return ttc