--- a/python/moving.py	Thu Jul 11 00:17:25 2013 -0400
+++ b/python/moving.py	Thu Jul 11 02:17:12 2013 -0400
@@ -30,6 +30,9 @@
     def empty(self):
         return self.first > self.last
 
+    def center(self):
+        return (self.first+self.last)/2.
+
     def length(self):
         '''Returns the length of the interval'''
         return float(max(0,self.last-self.first))
@@ -306,33 +309,39 @@
     def similar(f1, f2, maxDistance2, maxDeltavelocity2):
         return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2
 
+def intersection(p1, p2, dp1, dp2):
+    '''Returns the intersection point between the two lines 
+    defined by the respective vectors (dp) and origin points (p)'''
+    from numpy import matrix
+    from numpy.linalg import linalg
+    A = matrix([[dp1.y, -dp1.x],
+                [dp2.y, -dp2.x]])
+    B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
+                [dp2.y*p2.x-dp2.x*p2.y]])
+    
+    if linalg.det(A) == 0:
+        return None
+    else:
+        intersection = linalg.solve(A,B)
+        return Point(intersection[0,0], intersection[1,0])
+
 def segmentIntersection(p1, p2, p3, p4):
     '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise'''
-    from numpy import matrix
-    from numpy.linalg import linalg, det
 
     if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()):
         return None
     else:
-        dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]]
-        dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]]
-
-        A = matrix([[dp1.y, -dp1.x],
-                    [dp2.y, -dp2.x]])
-        B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
-                    [dp2.y*p3.x-dp2.x*p3.y]])
-
-        if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0:
+        dp1 = p2-p1
+        dp3 = p4-p3
+        inter = intersection(p1, p3, dp1, dp3)
+        if (inter != None 
+            and utils.inBetween(p1.x, p2.x, inter.x)
+            and utils.inBetween(p3.x, p4.x, inter.x)
+            and utils.inBetween(p1.y, p2.y, inter.y)
+            and utils.inBetween(p3.y, p4.y, inter.y)):
+            return inter
+        else:
             return None
-        else:
-            intersection = linalg.solve(A,B)
-            if (utils.inBetween(p1.x, p2.x, intersection[0,0])
-                and utils.inBetween(p3.x, p4.x, intersection[0,0])
-                and utils.inBetween(p1.y, p2.y, intersection[1,0])
-                and utils.inBetween(p3.y, p4.y, intersection[1,0])):
-                return Point(intersection[0,0], intersection[1,0])
-            else:
-                return None
 
 # TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection