Mercurial Hosting > traffic-intelligence

comparison python/moving.py @ 484:6464e4f0cc26

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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