Mercurial Hosting > traffic-intelligence

annotate TTC Sample/timeToCollision.py @ 462:af2222c0c9c0

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
TTC tested and updatet!
author Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
date Mon, 03 Feb 2014 15:41:57 -0500
parents
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
462
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
1 def timeToCollision(obj1,obj2,collisionDistanceThreshold,frameNum,framePerSecond):
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
2
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
3 import numpy as np
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
4
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
5 x1 = obj1.getPositionAtInstant(frameNum).x
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
6 y1 = obj1.getPositionAtInstant(frameNum).y
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
7 x2 = obj2.getPositionAtInstant(frameNum).x
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
8 y2 = obj2.getPositionAtInstant(frameNum).y
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
9 v1x = obj1.getVelocityAtInstant(frameNum).x * framePerSecond
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
10 v1y = obj1.getVelocityAtInstant(frameNum).y * framePerSecond
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
11 v2x = obj2.getVelocityAtInstant(frameNum).x * framePerSecond
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
12 v2y = obj2.getVelocityAtInstant(frameNum).y * framePerSecond
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
13 l = collisionDistanceThreshold
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
14
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
15 a = pow(v1x-v2x,2) + pow(v1y-v2y,2)
af2222c0c9c0 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))
af2222c0c9c0 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)
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
18
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
19 if pow(b,2) >= 4*a*c:
af2222c0c9c0 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)
af2222c0c9c0 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)
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
22 if ttc1 >= 0 and ttc2 >= 0:
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
23 ttc = min(ttc1,ttc2)
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
24 else:
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
25 if ttc1 < 0:
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
26 ttc = ttc2
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
27 if ttc2 < 0:
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
28 ttc = ttc1
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
29 if ttc1 < 0 and ttc2 < 0:
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
30 ttc = []
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
31 else:
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
32 ttc = []
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
33
af2222c0c9c0 TTC tested and updatet!
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
34 return ttc