annotate scripts/TTCcomputation.py @ 441:ea907ae19d8c

new TTC added
author Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
date Thu, 30 Jan 2014 16:35:17 -0500
parents
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
441
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
1 def TTC(x1,y1,x2,y2,v1x,v1y,v2x,v2y,l):
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
2
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
3 a = pow(v1x-v2x,2) + pow(v1y-v2y,2)
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
4 b = 2 * ((x1-x2) * (v1x-v2x) + (y1-y2) * (v1y-v2y))
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
5 c = pow(x1-x2,2) + pow(y1-y2,2) - pow(l,2)
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
6
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
7 if pow(b,2) >= 4*a*c:
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
8 ttc1 = (-b + sqrt(pow(b,2) - 4*a*c)) / (2*a)
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
9 ttc2 = (-b - sqrt(pow(b,2) - 4*a*c)) / (2*a)
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
10 if ttc1 >= 0 and ttc2 >= 0:
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
11 ttc = min(ttc1,ttc2)
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
12 else:
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
13 if ttc1 < 0:
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
14 ttc = ttc
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
15 if ttc2 < 0:
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
16 ttc = ttc1
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
17 if ttc1 < 0 and ttc2 < 0:
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
18 ttc = []
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
19 else:
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
20 ttc = []
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
21
ea907ae19d8c new TTC added
Sohail Zangenehpour <sohail.zangenehpour@mail.mcgill.ca>
parents:
diff changeset
22 return ttc