Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 504:a40c75f04903
optimized direct time to collision computation and added tests
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 02 Jun 2014 17:33:22 -0400 |
parents | c81cbd6953fb |
children | 727e3c529519 |
comparison
equal
deleted
inserted
replaced
503:7978b286fcfa | 504:a40c75f04903 |
---|---|
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, collisionThreshold): | 270 def timeToCollision(p1, p2, v1, v2, collisionThreshold): |
271 '''Computes exact time to collision with a distance threshold | |
272 The unknown of the equation is the time to reach the intersection | |
273 between the relative trajectory of one road user | |
274 and the circle of radius collisionThreshold around the other road user''' | |
271 from math import sqrt | 275 from math import sqrt |
272 a = (v1.x-v2.x)**2 + (v1.y-v2.y)**2 | 276 dv = v1-v2 |
273 b = 2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) | 277 dp = p1-p2 |
274 c = (p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 | 278 a = dv.norm2Squared()#(v1.x-v2.x)**2 + (v1.y-v2.y)**2 |
275 | 279 b = 2*dot(dv, dp)#2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) |
276 if b**2 >= 4*a*c: | 280 c = dp.norm2Squared() - collisionThreshold**2#(p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 |
277 ttc1 = (-b + sqrt(b**2 - 4*a*c)) / (2*a) | 281 |
278 ttc2 = (-b - sqrt(b**2 - 4*a*c)) / (2*a) | 282 delta = b**2 - 4*a*c |
283 if delta >= 0: | |
284 deltaRoot = sqrt(delta) | |
285 ttc1 = (-b + deltaRoot)/(2*a) | |
286 ttc2 = (-b - deltaRoot)/(2*a) | |
279 if ttc1 >= 0 and ttc2 >= 0: | 287 if ttc1 >= 0 and ttc2 >= 0: |
280 ttc = min(ttc1,ttc2) | 288 ttc = min(ttc1,ttc2) |
281 else: | 289 else: |
282 if ttc1 < 0: | 290 if ttc1 < 0: |
283 ttc = ttc2 | 291 ttc = ttc2 |