Mercurial Hosting > traffic-intelligence
diff 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 |
line wrap: on
line diff
--- a/python/moving.py Tue May 27 17:09:10 2014 -0400 +++ b/python/moving.py Mon Jun 02 17:33:22 2014 -0400 @@ -268,14 +268,22 @@ @staticmethod def timeToCollision(p1, p2, v1, v2, collisionThreshold): + '''Computes exact time to collision with a distance threshold + The unknown of the equation is the time to reach the intersection + between the relative trajectory of one road user + and the circle of radius collisionThreshold around the other road user''' from math import sqrt - a = (v1.x-v2.x)**2 + (v1.y-v2.y)**2 - b = 2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) - c = (p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 + dv = v1-v2 + dp = p1-p2 + a = dv.norm2Squared()#(v1.x-v2.x)**2 + (v1.y-v2.y)**2 + b = 2*dot(dv, dp)#2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) + c = dp.norm2Squared() - collisionThreshold**2#(p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 - if b**2 >= 4*a*c: - ttc1 = (-b + sqrt(b**2 - 4*a*c)) / (2*a) - ttc2 = (-b - sqrt(b**2 - 4*a*c)) / (2*a) + delta = b**2 - 4*a*c + if delta >= 0: + deltaRoot = sqrt(delta) + ttc1 = (-b + deltaRoot)/(2*a) + ttc2 = (-b - deltaRoot)/(2*a) if ttc1 >= 0 and ttc2 >= 0: ttc = min(ttc1,ttc2) else: