Mercurial Hosting > traffic-intelligence

changeset 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 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