--- a/python/moving.py	Tue Sep 06 17:55:06 2011 -0400
+++ b/python/moving.py	Tue Sep 06 18:44:23 2011 -0400
@@ -199,6 +199,31 @@
         from matplotlib.pyplot import scatter
         scatter([p.x for p in points],[p.y for p in points], c=color)
 
+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
+
+    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:
+        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
+
 class Trajectory:
     '''Class for trajectories
     i.e. a temporal sequence of positions