Mercurial > hg > nsaunier > traffic-intelligence

comparison scripts/timeToCollision.py @ 463:cb9683f9efe7

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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
comparison
equal deleted inserted replaced
460:55b424d98b68 463:cb9683f9efe7
1 def timeToCollision(obj1,obj2,collisionDistanceThreshold,frameNum,framePerSecond):
2
3 import numpy as np
4
5 x1 = obj1.getPositionAtInstant(frameNum).x
6 y1 = obj1.getPositionAtInstant(frameNum).y
7 x2 = obj2.getPositionAtInstant(frameNum).x
8 y2 = obj2.getPositionAtInstant(frameNum).y
9 v1x = obj1.getVelocityAtInstant(frameNum).x * framePerSecond
10 v1y = obj1.getVelocityAtInstant(frameNum).y * framePerSecond
11 v2x = obj2.getVelocityAtInstant(frameNum).x * framePerSecond
12 v2y = obj2.getVelocityAtInstant(frameNum).y * framePerSecond
13 l = collisionDistanceThreshold
14
15 a = pow(v1x-v2x,2) + pow(v1y-v2y,2)
16 b = 2 * ((x1-x2) * (v1x-v2x) + (y1-y2) * (v1y-v2y))
17 c = pow(x1-x2,2) + pow(y1-y2,2) - pow(l,2)
18
19 if pow(b,2) >= 4*a*c:
20 ttc1 = (-b + np.sqrt(pow(b,2) - 4*a*c)) / (2*a)
21 ttc2 = (-b - np.sqrt(pow(b,2) - 4*a*c)) / (2*a)
22 if ttc1 >= 0 and ttc2 >= 0:
23 ttc = min(ttc1,ttc2)
24 else:
25 if ttc1 < 0:
26 ttc = ttc2
27 if ttc2 < 0:
28 ttc = ttc1
29 if ttc1 < 0 and ttc2 < 0:
30 ttc = []
31 else:
32 ttc = []
33
34 return ttc