Mercurial Hosting > traffic-intelligence
annotate scripts/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 | cb41f9a4652b |
children |
rev | line source |
---|---|
461
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
1 def timeToCollision(obj1,obj2,collisionDistanceThreshold,frameNum,framePerSecond): |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
2 |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
3 import numpy as np |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
4 |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
5 x1 = obj1.getPositionAtInstant(frameNum).x |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
6 y1 = obj1.getPositionAtInstant(frameNum).y |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
7 x2 = obj2.getPositionAtInstant(frameNum).x |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
8 y2 = obj2.getPositionAtInstant(frameNum).y |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
9 v1x = obj1.getVelocityAtInstant(frameNum).x * framePerSecond |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
10 v1y = obj1.getVelocityAtInstant(frameNum).y * framePerSecond |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
11 v2x = obj2.getVelocityAtInstant(frameNum).x * framePerSecond |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
12 v2y = obj2.getVelocityAtInstant(frameNum).y * framePerSecond |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
13 l = collisionDistanceThreshold |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
14 |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
15 a = pow(v1x-v2x,2) + pow(v1y-v2y,2) |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
16 b = 2 * ((x1-x2) * (v1x-v2x) + (y1-y2) * (v1y-v2y)) |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
17 c = pow(x1-x2,2) + pow(y1-y2,2) - pow(l,2) |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
18 |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
19 if pow(b,2) >= 4*a*c: |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
20 ttc1 = (-b + np.sqrt(pow(b,2) - 4*a*c)) / (2*a) |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
21 ttc2 = (-b - np.sqrt(pow(b,2) - 4*a*c)) / (2*a) |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
22 if ttc1 >= 0 and ttc2 >= 0: |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
23 ttc = min(ttc1,ttc2) |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
24 else: |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
25 if ttc1 < 0: |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
26 ttc = ttc2 |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
27 if ttc2 < 0: |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
28 ttc = ttc1 |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
29 if ttc1 < 0 and ttc2 < 0: |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
30 ttc = [] |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
31 else: |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
32 ttc = [] |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
33 |
cb41f9a4652b
TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff
changeset
|
34 return ttc |