diff python/moving.py @ 464:dcc821b98efc

integrated and reorganized Sohail s work on exact ttc computation
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Sun, 23 Feb 2014 23:18:08 -0500
parents cd342a774806
children 6464e4f0cc26
line wrap: on
line diff
--- a/python/moving.py	Sun Feb 23 22:56:54 2014 -0500
+++ b/python/moving.py	Sun Feb 23 23:18:08 2014 -0500
@@ -266,6 +266,30 @@
         'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold'
         return Point.cosine(self, refDirection) >= cosineThreshold
 
+    @staticmethod
+    def timeToCollision(p1, p2, v1, v2):
+        from math import sqrt
+        a = pow(v1.x-v2.x,2) + pow(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)
+
+        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 ttc1 >= 0 and ttc2 >= 0:
+                ttc = min(ttc1,ttc2)
+            else:
+                if ttc1 < 0:
+                    ttc = ttc2
+                if ttc2 < 0:
+                    ttc = ttc1
+                if ttc1 < 0 and ttc2 < 0:
+                    ttc = None
+        else:
+            ttc = None
+        return ttc
+
+
 if shapelyAvailable:
     def pointsInPolygon(points, polygon):
         '''Optimized tests of a series of points within (Shapely) polygon '''