--- a/python/utils.py	Sat Dec 05 12:31:28 2009 -0500
+++ b/python/utils.py	Sat Dec 05 15:40:28 2009 -0500
@@ -4,49 +4,54 @@
 #from numpy import *
 #from pylab import *
 
+from moving import Point,Interval
+
 __metaclass__ = type
 
 commentChar = '#';
 
-# maths
+#########################
+# maths section
+#########################
 
-def segmentIntersection(s1, s2):
-    '''Returns the intersecting point, None otherwise
-    format for a segment is [[x1, x2], [y1,y2]]
+def segmentIntersection(p1, p2, p3, p4):
+    '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise
     
-    >>> segmentIntersection([[0, 1], [0,1]], [[0, 1], [1,2]])
-    >>> segmentIntersection([[0, 1], [1,0]], [[0, 1], [2,1]])
-    >>> segmentIntersection([[0, 2], [0,0]], [[1, 1], [-1,1]])
-    [1.0, 0.0]
-    >>> segmentIntersection([[0, 2], [0,0]], [[1, 1], [1,2]])
+    >>> segmentIntersection(Point(0,0),Point(1,1), Point(0,1), Point(1,2))
+    >>> segmentIntersection(Point(0,1),Point(1,0), Point(0,2), Point(2,1))
+    >>> segmentIntersection(Point(0,0),Point(2,0), Point(1,-1),Point(1,1))
+    (1.000000,0.000000)
+    >>> segmentIntersection(Point(0,1),Point(2,0),Point(1,1),Point(1,2))
     '''
     from numpy import matrix
     from numpy.linalg import linalg, det
 
-    ds1 = [s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]]
-    ds2 = [s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]]
+    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([[ds1[1], -ds1[0]],
-                [ds2[1], -ds2[0]]])
-    B = matrix([[ds1[1]*s1[0][0]-ds1[0]*s1[1][0]],
-                [ds2[1]*s2[0][0]-ds2[0]*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 (intersection[0,0] >= s1[0][0] and intersection[0,0] <= s1[0][1]
-            and intersection[0,0] >= s2[0][0] and intersection[0,0] <= s2[0][1]
-            and intersection[1,0] >= s1[1][0] and intersection[1,0] <= s1[1][1]
-            and intersection[1,0] >= s2[1][0] and intersection[1,0] <= s2[1][1]):
-            return [intersection[0,0], intersection[1,0]]
+        if (Interval(p1.x, p2.x, True).contains(intersection[0,0])
+            and Interval(p3.x, p4.x, True).contains(intersection[0,0])
+            and Interval(p1.y, p2.y, True).contains(intersection[1,0])
+            and Interval(p3.y, p4.y, True).contains(intersection[1,0])):
+            return Point(intersection[0,0], intersection[1,0])
         else:
             return None
 
 def crossProduct(l1, l2):
     return l1[0]*l2[1]-l1[1]*l2[0]
 
-# file I/O
+#########################
+# file I/O section
+#########################
 
 def openCheck(filename, option = 'r', quit = False):
     '''Open file filename in read mode by default
@@ -103,6 +108,7 @@
 
 def invertHomography(homography):
     'Returns an inverted homography'
+    from numpy.linalg.linalg import inv
     invH = inv(homography)
     invH /= invH[2,2]
     return invH
@@ -122,8 +128,19 @@
         projected = p
     return projected[:2]
 
-def printPoint(x,y):
-    return '(%f,%f)'%(x,y)
+def projectTrajectory(homography, trajectory):
+    '''Projects a series of points in the format
+    [[x1, x2, ...],
+    [y1, y2, ...]]
+
+    Warning: not optimized, calls project()'''
+    projected = [[],[]]
+    for x, y in zip(trajectory[0], trajectory[1]):
+        p = [x,y]
+        pp = project(homography, p)
+        projected[0].append(pp[0])
+        projected[1].append(pp[1])
+    return projected
 
 def plotPolygon(poly, options = ''):
     from numpy.core.multiarray import array