Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 484:6464e4f0cc26
integrated Sohail direct computation of TTC (need to add pPET)
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 02 Apr 2014 17:45:18 -0400 |
parents | dcc821b98efc |
children | c81cbd6953fb |
comparison
equal
deleted
inserted
replaced
483:30b3455978d9 | 484:6464e4f0cc26 |
---|---|
265 def similarOrientation(self, refDirection, cosineThreshold): | 265 def similarOrientation(self, refDirection, cosineThreshold): |
266 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold' | 266 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold' |
267 return Point.cosine(self, refDirection) >= cosineThreshold | 267 return Point.cosine(self, refDirection) >= cosineThreshold |
268 | 268 |
269 @staticmethod | 269 @staticmethod |
270 def timeToCollision(p1, p2, v1, v2): | 270 def timeToCollision(p1, p2, v1, v2, collisionThreshold): |
271 from math import sqrt | 271 from math import sqrt |
272 a = pow(v1.x-v2.x,2) + pow(v1.y-v2.y,2) | 272 a = (v1.x-v2.x)**2 + (v1.y-v2.y)**2 |
273 b = 2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) | 273 b = 2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) |
274 c = pow(p1.x-p2.x,2) + pow(p1.y-p2.y,2) - pow(l,2) | 274 c = (p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 |
275 | 275 |
276 if pow(b,2) >= 4*a*c: | 276 if b**2 >= 4*a*c: |
277 ttc1 = (-b + sqrt(pow(b,2) - 4*a*c)) / (2*a) | 277 ttc1 = (-b + sqrt(b**2 - 4*a*c)) / (2*a) |
278 ttc2 = (-b - sqrt(pow(b,2) - 4*a*c)) / (2*a) | 278 ttc2 = (-b - sqrt(b**2 - 4*a*c)) / (2*a) |
279 if ttc1 >= 0 and ttc2 >= 0: | 279 if ttc1 >= 0 and ttc2 >= 0: |
280 ttc = min(ttc1,ttc2) | 280 ttc = min(ttc1,ttc2) |
281 else: | 281 else: |
282 if ttc1 < 0: | 282 if ttc1 < 0: |
283 ttc = ttc2 | 283 ttc = ttc2 |
284 if ttc2 < 0: | 284 elif ttc2 < 0: |
285 ttc = ttc1 | 285 ttc = ttc1 |
286 if ttc1 < 0 and ttc2 < 0: | 286 else: # ttc1 < 0 and ttc2 < 0: |
287 ttc = None | 287 ttc = None |
288 else: | 288 else: |
289 ttc = None | 289 ttc = None |
290 return ttc | 290 return ttc |
291 | 291 |