Mercurial Hosting > traffic-intelligence
changeset 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 | 30b3455978d9 |
children | a5847c0ca27c |
files | python/moving.py python/prediction.py |
diffstat | 2 files changed, 16 insertions(+), 14 deletions(-) [+] |
line wrap: on
line diff
--- a/python/moving.py Wed Apr 02 17:25:50 2014 -0400 +++ b/python/moving.py Wed Apr 02 17:45:18 2014 -0400 @@ -267,23 +267,23 @@ return Point.cosine(self, refDirection) >= cosineThreshold @staticmethod - def timeToCollision(p1, p2, v1, v2): + def timeToCollision(p1, p2, v1, v2, collisionThreshold): from math import sqrt - a = pow(v1.x-v2.x,2) + pow(v1.y-v2.y,2) + 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 = pow(p1.x-p2.x,2) + pow(p1.y-p2.y,2) - pow(l,2) + c = (p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 - if pow(b,2) >= 4*a*c: - ttc1 = (-b + sqrt(pow(b,2) - 4*a*c)) / (2*a) - ttc2 = (-b - sqrt(pow(b,2) - 4*a*c)) / (2*a) + 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) if ttc1 >= 0 and ttc2 >= 0: ttc = min(ttc1,ttc2) else: if ttc1 < 0: ttc = ttc2 - if ttc2 < 0: + elif ttc2 < 0: ttc = ttc1 - if ttc1 < 0 and ttc2 < 0: + else: # ttc1 < 0 and ttc2 < 0: ttc = None else: ttc = None
--- a/python/prediction.py Wed Apr 02 17:25:50 2014 -0400 +++ b/python/prediction.py Wed Apr 02 17:45:18 2014 -0400 @@ -400,12 +400,14 @@ p2 = obj2.getPositionAtInstant(currentInstant) v1 = obj1.getVelocityAtInstant(currentInstant) v2 = obj2.getVelocityAtInstant(currentInstant) - - ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) - if ttc: - return [SafetyPoint(moving.Point(), 1., ttc)], [] - else: - return [],[] + intersection = moving.intersection(p1, p2, v1, v2) + + if intersection != None: + ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) + if ttc: + return [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5) + else: + return [],[] #### # Other Methods