Mercurial Hosting > traffic-intelligence
changeset 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 | 7978b286fcfa |
children | 35c99776e593 |
files | python/moving.py python/tests/moving.txt |
diffstat | 2 files changed, 23 insertions(+), 6 deletions(-) [+] |
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:
--- a/python/tests/moving.txt Tue May 27 17:09:10 2014 -0400 +++ b/python/tests/moving.txt Mon Jun 02 17:33:22 2014 -0400 @@ -85,6 +85,15 @@ >>> Trajectory.lcss(t1, t1, lcss) 3 +>>> p1=Point(0,0) +>>> p2=Point(1,0) +>>> v1 = Point(0.1,0.1) +>>> v2 = Point(-0.1, 0.1) +>>> abs(Point.timeToCollision(p1, p2, v1, v2, 0.)-5.0) < 0.00001 +True +>>> abs(Point.timeToCollision(p1, p2, v1, v2, 0.1)-4.5) < 0.00001 +True + >>> o1 = MovingObject(positions = Trajectory([[0]*3,[2]*3]), velocities = Trajectory([[0]*3,[1]*3])) >>> o1.classifyUserTypeSpeed(0.5, np.median) >>> userTypeNames[o1.getUserType()]