Mercurial Hosting > traffic-intelligence
diff scripts/timeToCollision.py @ 463:cb9683f9efe7
Merged in szangenehpour/trafficintelligence (pull request #5)
TTC Sample
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Sun, 23 Feb 2014 22:56:54 -0500 |
parents | cb41f9a4652b |
children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/scripts/timeToCollision.py Sun Feb 23 22:56:54 2014 -0500 @@ -0,0 +1,34 @@ +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