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